#include <Arduino.h>

#ifdef SEEED_XIAO_M0
HardwareSerial *pSerial = &Serial1;
#define PSERIAL SerialUSB
#endif
#ifdef CONFIG_IDF_TARGET_ESP32
HardwareSerial *pSerial = &Serial2;
#define PSERIAL Serial
#endif

#define JDBG_BAUD 115200
#define P(...) PSERIAL.print(__VA_ARGS__)
#define PN(...) PSERIAL.println(__VA_ARGS__)

#define MAXLEN 128

static uint8_t packet[MAXLEN];  // data read from Master

static uint32_t crcError;
static uint32_t unknownCnt;

static uint16_t crc_ccitt_update(uint16_t crc, uint8_t data) {
  uint16_t ret_val;
  data ^= (uint8_t)(crc) & (uint8_t)(0xFF);
  data ^= data << 4;
  ret_val = ((((uint16_t)data << 8) | ((crc & 0xFF00) >> 8))
             ^ (uint8_t)(data >> 4)
             ^ ((uint16_t)data << 3));
  return ret_val;
}

static uint16_t get_crc16z(uint8_t *p, uint16_t len) {
  uint16_t crc16_data = 0;
  while (len--) {
    crc16_data = crc_ccitt_update(crc16_data, p[0]);
    p++;
  }
  return (crc16_data);
}

static bool checkPacketCrc(uint8_t *pkt) {
  uint8_t i = pkt[2] - 2;
  uint16_t crc = get_crc16z((uint8_t *)pkt, i);
  uint16_t crcInPkt = (uint16_t)pkt[i] + (((uint16_t)pkt[i + 1]) << 8);

  if (crc == crcInPkt)
    return true;
  else
    crcError++;
  return false;
}

// 0:0x3D 1:0x1 2:0x8 3:0x58 4:0x40 5:0x0 6:0xD4 7:0x1F
// 0:0x3D 1:0x1 2:0x8 3:0x68 4:0x40 5:0x0 6:0x7A 7:0x99
static void decodeMaster(uint8_t val) {
  static uint8_t i = 0;
  static unsigned long t = millis();

  packet[i < MAXLEN ? i : 0] = val;
  switch (i) {
    case 0:
      i = val == 0x3D ? 1 : 0;
      return;
    case 1:
      i = val == 0x01 ? 2 : 0;
      return;
    case 2:
      i = val == 0x08 ? 3 : 0;
      return;
    case 3:  // Byte 4  Packet_ID
      i = 4;
      return;
    case 4:  // Byte 5 DATA_ID1 The data identifier 1
      if (val == 0x40 && packet[0] == 0x3D && packet[1] == 0x01 && packet[2] == 8) {
        // A packet not documented (crc is valid): 0x3D 0x01 0x08 ID 0x40 0x00 Crc16-1 Crc16-2
        i = 5;
      } else {
        i = 0;
      }
      return;
    case 5:  // Byte 6 SUB_LEN
      i = (val == 0x0) ? 6 : 0;
      return;
    case 6:  // crc
      i = 7;
      return;
    case 7:  // crc
      i = 8;
      return;
    case 8:  // crc
      i = 0;
      if (checkPacketCrc(packet)) {
        unknownCnt++;
        P(millis() - t);  // period
        P(":cnt:");
        P(unknownCnt);  // number of occurences
        P(":");
        for (uint8_t i = 0; i < packet[2]; i++) {  // dump packet
          P(i);
          P(":0x");
          PSERIAL.print(packet[i], HEX);
          P(" ");
        }
        PN("");
        t = millis();
        return;
      }
    default:
      break;
  }
}

void setup() {
  PSERIAL.begin(JDBG_BAUD);
  delay(5000);
  PN("setup");
#ifdef SEEED_XIAO_M0
  pSerial->begin(125000, SERIAL_8N1);  // default Rx Pin is D7
#endif
#ifdef CONFIG_IDF_TARGET_ESP32
  pSerial->begin(125000, SERIAL_8N1, 19, 18);  // RX pin is 19
#endif
  delay(1000);
}

void loop() {
  if (pSerial->available() > 0)
    (void)decodeMaster((uint8_t)pSerial->read());
}