#include "flash.h"

#include "devicelib.h"

#include <pico/stdlib.h>

using namespace lib;

Flash::Page::operator unsigned char*() { return buffer; }
Flash::Page::operator const unsigned char*() const { return buffer; }

uint8_t& Flash::Page::operator[](int address) { return buffer[address]; }
uint8_t Flash::Page::operator[](int address) const { return buffer[address]; }

const uint8_t* Flash::Page::begin() const { return buffer; }
const uint8_t* Flash::Page::end() const { return buffer + SIZE; }

uint8_t* Flash::Page::begin() { return buffer; }
uint8_t* Flash::Page::end() { return buffer + SIZE; }

Flash::Flash(uint8_t sck, uint8_t tx, uint8_t rx, uint8_t cs, int baudrate) :
  sck(sck),
  tx(tx),
  rx(rx),
  cs(cs)
{
  spi_inst_t* sck_spi = detail::get_spi_instance(sck);
  spi_inst_t* tx_spi = detail::get_spi_instance(tx);
  spi_inst_t* rx_spi = detail::get_spi_instance(rx);

  hard_assert(sck_spi == tx_spi && tx_spi == rx_spi, "Invalid configuration");
  spi = sck_spi;

  gpio_set_function(sck, GPIO_FUNC_SPI);
  gpio_set_function(tx, GPIO_FUNC_SPI);
  gpio_set_function(rx, GPIO_FUNC_SPI);

  gpio_set_function(cs, GPIO_FUNC_SIO);
  gpio_set_dir(cs, GPIO_OUT);
  gpio_put(cs, true);

  spi_init(spi, baudrate);
}

void Flash::read_page(uint16_t page_index)
{
  const uint8_t cmd[4] = { CMD_READ, static_cast<uint8_t>(page_index >> 8),
                           static_cast<uint8_t>(page_index), 0 };

  gpio_put(cs, false);
  spi_write_blocking(spi, cmd, 4);
  spi_read_blocking(spi, 0, page_buffer, Page::SIZE);
  gpio_put(cs, true);
}

void Flash::write_page(uint16_t page_index)
{
  write_enable();

  uint8_t cmd[4] = { CMD_PAGE_PROGRAM, static_cast<uint8_t>(page_index >> 8),
                     static_cast<uint8_t>(page_index), 0 };

  gpio_put(cs, false);
  spi_write_blocking(spi, cmd, 4);
  spi_write_blocking(spi, page_buffer, Page::SIZE);
  gpio_put(cs, true);

  wait_done();
}

void Flash::erase_sector(uint16_t page_index)
{
  write_enable();

  uint8_t cmd[4] = { CMD_SECTOR_ERASE, static_cast<uint8_t>(page_index >> 8),
                     static_cast<uint8_t>(page_index), 0 };

  gpio_put(cs, false);
  spi_write_blocking(spi, cmd, 4);
  gpio_put(cs, true);

  wait_done();
}

Flash::Page& Flash::page() { return page_buffer; }
const Flash::Page& Flash::page() const { return page_buffer; }

void Flash::write_enable()
{
  uint8_t cmd = CMD_WRITE_EN;

  gpio_put(cs, false);
  spi_write_blocking(spi, &cmd, 1);
  gpio_put(cs, true);
  sleep_us(100);
}

void Flash::wait_done()
{
  for (uint8_t status = STATUS_BUSY_MASK; status & STATUS_BUSY_MASK;)
  {
    uint8_t cmd = CMD_STATUS;

    gpio_put(cs, false);
    spi_write_blocking(spi, &cmd, 1);
    spi_read_blocking(spi, 0, &status, 1);
    gpio_put(cs, true);
  }
}
