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

#include "hardware/spi.h"

#include "spi_parser.h"

#define AT86_RSTN 7

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
  // to set MODE to read
  cs_select();
  spi_write_blocking(PICO_DEFAULT_SPI, cmd, 2);
  spi_write_blocking(PICO_DEFAULT_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
  cmd[0] |= (2 << 6);
  // set MODE[0] to set to write 
  cs_select();
  spi_write_blocking(PICO_DEFAULT_SPI, cmd, 2);
  spi_read_blocking(PICO_DEFAULT_SPI, 0, buf, len);
  cs_deselect();
}

int main() {
  stdio_init_all();
  // 10 MHz
  spi_init(PICO_DEFAULT_SPI, 10*1000*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_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 == 0x0a) {
          cmd[cmd_idx] = 0;
          putchar(maybe_ch);
          // process command
          parse_and_execute_cmd(cmd, cmd_idx);
          cmd_idx = 0;
        } else if (maybe_ch == 0x1b) {
          putchar('^');
          putchar('\n');
          cmd_idx = 0;
        } else {
          putchar(maybe_ch);
          cmd[cmd_idx] = maybe_ch;
          if (cmd_idx < sizeof(cmd)-1) {
            ++cmd_idx;
          }
        }
      }
    } else {
      sleep_ms(10);
    }
  }
}
