#include <stdio.h>
#include "pico/stdlib.h"

#include "hardware/spi.h"

#include "spi_parser.h"

#define AT86_RSTN 5

#define SPI spi0

static void cs_select() {
  asm volatile("nop \n nop \n nop");
  gpio_put(PICO_DEFAULT_SPI_CSN_PIN, 0);  // Active low
  asm volatile("nop \n nop \n nop");
}

static void cs_deselect() {
  asm volatile("nop \n nop \n nop");
  gpio_put(PICO_DEFAULT_SPI_CSN_PIN, 1);
  asm volatile("nop \n nop \n nop");
}

static void spi_register_write(uint16_t address, const uint8_t *buf, uint16_t len) {
  uint8_t cmd[2] = { address >> 8, address };
  cmd[0] &= ~(3 << 6); // mask out top two bits
  cmd[0] |= (2 << 6);
  // set MODE[0] to set to write 
  // to set MODE to read
  cs_select();
  spi_write_blocking(SPI, cmd, 2);
  spi_write_blocking(SPI, buf, len);
  cs_deselect();
}

static void spi_register_read(uint16_t address, uint8_t *buf, uint16_t len) {
  uint8_t cmd[2] = { address >> 8, address };
  cmd[0] &= ~(3 << 6); // mask out top two bits
  cs_select();
  spi_write_blocking(SPI, cmd, 2);
  spi_read_blocking(SPI, 0x00, buf, len);
  cs_deselect();
}

int main() {
  stdio_init_all();
  // 10 MHz
  spi_init(SPI, 100*1000);
  gpio_set_function(PICO_DEFAULT_SPI_RX_PIN, GPIO_FUNC_SPI);
  gpio_set_function(PICO_DEFAULT_SPI_SCK_PIN, GPIO_FUNC_SPI);
  gpio_set_function(PICO_DEFAULT_SPI_TX_PIN, GPIO_FUNC_SPI);

  gpio_init(PICO_DEFAULT_SPI_CSN_PIN);
  gpio_set_dir(PICO_DEFAULT_SPI_CSN_PIN, GPIO_OUT);
  gpio_put(PICO_DEFAULT_SPI_CSN_PIN, 1);

  gpio_init(AT86_RSTN);
  gpio_set_dir(AT86_RSTN, GPIO_OUT);
  gpio_put(AT86_RSTN, 1);

  uint8_t cmd[64];
  int cmd_idx = 0;
  while (true) {
    if (stdio_usb_connected()) {
      // UART parse loop
      int maybe_ch = getchar_timeout_us(1000);
      if (maybe_ch != PICO_ERROR_TIMEOUT) {
        if ((maybe_ch != 0x1b) && (maybe_ch != 0x0d)) {
          putchar(maybe_ch);
        }
        //const char tohex[] = "0123456789abcdef";
        //putchar(tohex[(maybe_ch >> 4) & 0xf]);
        //putchar(tohex[maybe_ch & 0xf]);
        if (maybe_ch == 0x0d) {
          puts("\r\n");
          cmd[cmd_idx] = 0;
          ++cmd_idx;
          // process command
          /*
          puts("sending cmd ");
          for (int i = 0; i < cmd_idx; i++) {
            const char tohex[] = "0123456789abcdef";
            putchar(tohex[(cmd[i] >> 4) & 0xf]);
            putchar(tohex[cmd[i] & 0xf]);
          }
          */
          parse_and_execute_cmd(cmd, cmd_idx);
          puts("cmd finished\n");
          cmd_idx = 0;
        } else if (maybe_ch == 0x1b) {
          cmd_idx = 0;
        } else {
          cmd[cmd_idx] = maybe_ch;
          if (cmd_idx < sizeof(cmd)-1) {
            ++cmd_idx;
          }
        }
      }
    } else {
      sleep_ms(10);
    }
  }
}
