#include <EasyESP32CAN.h>

/*
  BEANalyzer UltraPro - Arduino/ESP32 CAN Serial Bridge

  Uses the EasyMultiCAN/EasyESP32CAN-style API:
    CAN.begin(...)
    CAN.sendMsgBuf(...)
    CAN.checkReceive()
    CAN.readMsgBuf(...)

  PC -> Arduino CAN TX:
    CTX,<can_id_hex>,<extended 0/1>,<dlc>,<hex bytes>

  Arduino -> PC CAN RX:
    CAN,<millis>,RX,<can_id_hex>,<extended 0/1>,<dlc>,<hex bytes>
*/

static const gpio_num_t CAN_TX_PIN = GPIO_NUM_6;
static const gpio_num_t CAN_RX_PIN = GPIO_NUM_7;
#define SERIAL_BAUD 115200
#define CAN_SPEED CAN_500KBPS

String serialLine;

uint8_t parseHexByte(const String &s) {
  return (uint8_t) strtoul(s.c_str(), nullptr, 16);
}

void printHexByte(uint8_t value) {
  if (value < 0x10) Serial.print('0');
  Serial.print(value, HEX);
}

bool parseHexPayload(String payload, uint8_t *out, uint8_t *outLen, uint8_t maxLen) {
  payload.replace(",", " ");
  payload.replace("0x", "");
  payload.replace("0X", "");
  payload.trim();

  uint8_t count = 0;
  int start = 0;

  while (start < payload.length()) {
    while (start < payload.length() && payload[start] == ' ') start++;
    if (start >= payload.length()) break;

    int end = payload.indexOf(' ', start);
    if (end < 0) end = payload.length();

    if (count >= maxLen) return false;
    out[count++] = parseHexByte(payload.substring(start, end));
    start = end + 1;
  }

  *outLen = count;
  return true;
}

void handleSerialCommand(String line) {
  line.trim();
  if (!line.length()) return;

  if (!line.startsWith("CTX,")) {
    Serial.println("ERR,UNKNOWN_COMMAND");
    return;
  }

  int p1 = line.indexOf(',', 4);
  int p2 = line.indexOf(',', p1 + 1);
  int p3 = line.indexOf(',', p2 + 1);

  if (p1 < 0 || p2 < 0 || p3 < 0) {
    Serial.println("ERR,CTX_FORMAT");
    return;
  }

  uint32_t canId = strtoul(line.substring(4, p1).c_str(), nullptr, 16);
  uint8_t ext = (uint8_t) line.substring(p1 + 1, p2).toInt();
  uint8_t dlc = (uint8_t) line.substring(p2 + 1, p3).toInt();
  String payload = line.substring(p3 + 1);

  uint8_t data[8];
  uint8_t len = 0;

  if (!parseHexPayload(payload, data, &len, sizeof(data))) {
    Serial.println("ERR,CTX_PAYLOAD");
    return;
  }

  if (dlc > 8) dlc = 8;
  if (len < dlc) dlc = len;

  INT8U rc = CAN.sendMsgBuf(canId, ext ? CAN_EXTID : CAN_STDID, dlc, data);

  Serial.print("OK,CTX,");
  Serial.print(rc);
  Serial.print(",");
  Serial.print(canId, HEX);
  Serial.print(",");
  Serial.print(ext);
  Serial.print(",");
  Serial.print(dlc);
  Serial.print(",");
  for (uint8_t i = 0; i < dlc; i++) {
    printHexByte(data[i]);
    if (i < dlc - 1) Serial.print(' ');
  }
  Serial.println();
}

void setup() {
  Serial.begin(SERIAL_BAUD);
  delay(300);

  if (CAN.begin(CAN_SPEED, CAN_TX_PIN, CAN_RX_PIN) != CAN_OK) {
    Serial.println("ERR,CAN_INIT_FAILED");
    while (true) delay(1000);
  }

  Serial.println("CAN_SERIAL_BRIDGE_READY");
}

void loop() {
  while (Serial.available()) {
    char c = (char)Serial.read();
    if (c == '\n' || c == '\r') {
      if (serialLine.length()) {
        handleSerialCommand(serialLine);
        serialLine = "";
      }
    } else {
      serialLine += c;
      if (serialLine.length() > 140) serialLine = "";
    }
  }

  if (CAN.checkReceive() == CAN_MSGAVAIL) {
    uint8_t len = 0;
    uint8_t data[8];
    if (CAN.readMsgBuf(&len, data) == CAN_OK) {
      Serial.print("CAN,");
      Serial.print(millis());
      Serial.print(",RX,");
      Serial.print(CAN.getCanId(), HEX);
      Serial.print(",");
      Serial.print(CAN.isExtendedFrame() ? 1 : 0);
      Serial.print(",");
      Serial.print(len);
      Serial.print(",");
      for (uint8_t i = 0; i < len; i++) {
        printHexByte(data[i]);
        if (i < len - 1) Serial.print(' ');
      }
      Serial.println();
    }
  }
}
