#ifndef SPI_PARSER_H_
#define SPI_PARSER_H_

#include <stdbool.h>

// probably should include stdint but I'm not sure how that'd
// interact with Pico SDK's stuff, so I'll leave it out

static int maybe_nibble(char c) {
  if ((c >= 'a') && (c <= 'f')) {
    return c - 'a' + 10;
  }
  if ((c >= '0') && (c <= '9')) {
    return c - '0';
  }
  return -1;
}


static void spi_register_write(uint16_t address, const uint8_t *buf, uint16_t len);
static void spi_register_read(uint16_t address, uint8_t *buf, uint16_t len);

static void parse_and_execute_cmd(const uint8_t* s, int c_len) {
  if (c_len == 0) {
    return;
  }

  int is_write = (s[0] == 'w');
  int is_read = (s[0] == 'r');
  if (!is_write && !is_read) {
    puts("invalid cmd; no r/w");
    return;
  }

  int offset = 1;

  uint16_t addr = 0;
  int len = 0;
  uint8_t buf[64];

  for (int i = 0; i < 4 && (i + 1) < c_len; i++) {
    int mn = maybe_nibble(s[i + offset]);
    if (mn < 0) {
      puts("invalid cmd; bad addr");
      return;
    }
    addr = (addr << 4) | (mn & 0xf);
  }

  offset += 4;
  offset += 1; // skip a character, some kind of whitespace is allowed
  // variable length entry, read until 
  int readlen = 0;
  int mn = maybe_nibble(s[offset]);
  while ((mn >= 0) && (offset < c_len)) {
    len = (len << 4) | (mn & 0xf);
    ++offset;
    mn = maybe_nibble(s[offset]);
  }

  if (len > sizeof(buf)) {
    len = sizeof(buf);
  }

  const uint8_t tohex[] = "0123456789abcdef";

  if (is_read) {
    // we're good, time to execute
    spi_register_read(addr, buf, len);
    uint8_t output_str[3*sizeof(buf) + 1];
    for (int i = 0; i < len; i++) {
      output_str[3*i] = tohex[(buf[i] >> 4) & 0xf];
      output_str[3*i + 1] = tohex[buf[i] & 0xf];
      output_str[3*i + 2] = ' ';
    }
    output_str[3*len - 1] = '\n';
    output_str[3*len] = 0;
    puts(output_str);
    return;
  }
  ++offset; // skip whitespace character
  
  if (is_write) {
    // parse write data one nibble at a time until it reaches the end
    // or len characters have been read
    int nibble_count = 0;
    int cur_byte = 0;
    for (; (nibble_count < 2*len) && (offset < c_len); offset++) {
      int mn = maybe_nibble(s[offset]);
      if (mn >= 0) {
        cur_byte = (cur_byte << 4) | (mn & 0xf);
        ++nibble_count;
        if ((nibble_count % 2) == 0) {
          buf[(nibble_count/2) - 1] = cur_byte & 0xff;
        }
      }
    }

    if (nibble_count < 2*len) {
      puts("parse error: not enough data for write\n");
      return;
    }

    spi_register_write(addr, buf, len);
    uint8_t output_s[3*sizeof(buf) + 4 + 9];
    output_s[0] = 'w';
    output_s[1] = ' ';
    for (int i = 0; i < 4; i++) {
      output_s[2+i] = tohex[(addr >> (4*(3-i))) & 0xf];
    }
    output_s[2+4] = ' ';

    uint8_t* output_str = output_s + 7;
    for (int i = 0; i < len; i++) {
      output_str[3*i] = tohex[(buf[i] >> 4) & 0xf];
      output_str[3*i + 1] = tohex[buf[i] & 0xf];
      output_str[3*i + 2] = ' ';
    }
    output_str[3*len - 1] = '\n';
    output_str[3*len] = 0;

    puts(output_s);
    return;
  }
}


#endif // SPI_PARSER_H_
