#include <BeanMPX.h>

/*
  BEANalyzer UltraPro - BEAN MPX Serial Bridge

  RX output to PC:
    B,<millis>,<type>,<len>,<hex bytes>

  TX input from PC:
    BTX,<hex bytes>

  Example:
    BTX,62 D2 10 00 00
*/

#define RX_BUFFER_SIZE 32
#define BEAN_RX_PIN 8
#define BEAN_TX_PIN 9
#define ENABLE_ACK false

BeanMPX bean;
uint8_t beanRxBuffer[RX_BUFFER_SIZE];
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 count > 0;
}

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

  if (line.startsWith("BTX,")) {
    uint8_t data[16];
    uint8_t len = 0;
    String payload = line.substring(4);

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

    if (bean.isBusy()) {
      Serial.println("ERR,BEAN_BUSY");
      return;
    }

    bean.sendMsg(data, len);
    Serial.print("OK,BTX,");
    Serial.print(len);
    Serial.print(",");
    for (uint8_t i = 0; i < len; i++) {
      printHexByte(data[i]);
      if (i < len - 1) Serial.print(' ');
    }
    Serial.println();
    return;
  }

  Serial.println("ERR,UNKNOWN_COMMAND");
}

void setup() {
  Serial.begin(115200);
  while (!Serial) { ; }

#if ENABLE_ACK
  bean.ackMsg((const uint8_t[]) {0xFE}, 1);
#endif

  bean.begin(BEAN_RX_PIN, BEAN_TX_PIN);
  Serial.println("BEAN_MPX_CAPTURE_TX_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() > 120) serialLine = "";
    }
  }

  if (!bean.available()) return;

  bean.getMsg(beanRxBuffer, RX_BUFFER_SIZE);

  uint8_t frameLen = beanRxBuffer[0];
  char msgType = (char)beanRxBuffer[1];

  if (frameLen > RX_BUFFER_SIZE - 2) frameLen = RX_BUFFER_SIZE - 2;

  Serial.print("B,");
  Serial.print(millis());
  Serial.print(",");
  Serial.print(msgType);
  Serial.print(",");
  Serial.print(frameLen);
  Serial.print(",");

  for (uint8_t i = 0; i < frameLen; i++) {
    printHexByte(beanRxBuffer[i + 2]);
    if (i < frameLen - 1) Serial.print(' ');
  }
  Serial.println();

  memset(beanRxBuffer, 0, RX_BUFFER_SIZE);
}
