#include "ssd1306.h"

#include <hardware/gpio.h>
#include <pico/error.h>

int ssd_init(ssd1306_t *display, i2c_inst_t *i2c, uint8_t addr) {
  display->i2c = i2c;
  display->addr = addr;
  display->len = 0;

  return PICO_OK;
}

int ssd_push(ssd1306_t *display, uint8_t byte) {
  if (display->len < SSD1306_BUFFER_LEN) {
    display->buf[display->len] = byte;
    display->len++;
    return PICO_OK;
  } else {
    return PICO_ERROR_BUFFER_TOO_SMALL;
  }
}

int ssd_send(ssd1306_t *display) {
  int ret = i2c_write_blocking(
    display->i2c,
    display->addr,
    display->buf,
    display->len,
    false
  );
  display->len = 0;

  return ret;
}

int ssd_power_on(ssd1306_t *display) {
  ssd_push(display, SSD1306_CTRL_CMD);
  
  ssd_push(display, SSD1306_DISPLAY_OFF);
  
  ssd_push(display, SSD1306_SET_CLOCK_DIV);
  ssd_push(display, 0x80);

  ssd_push(display, SSD1306_SET_MULTIPLEX);
  ssd_push(display, 0x1f);

  ssd_push(display, SSD1306_START_LINE(0));

  ssd_push(display, SSD1306_SET_CHARGE_PUMP);
  ssd_push(display, SSD1306_CHARGE_PUMP_ON);

  ssd_push(display, SSD1306_SET_MEMORY_MODE);
  ssd_push(display, SSD1306_MEM_MODE_HORIZONTAL);

  ssd_push(display, SSD1306_SEG_REMAP_COL127);
  ssd_push(display, SSD1306_COM_SCAN_REMAP);
  ssd_push(display, SSD1306_COM_PINS_SEQUENTIAL);

  ssd_push(display, SSD1306_SET_CONTRAST);
  ssd_push(display, 0xcf);

  ssd_push(display, SSD1306_SET_PRECHARGE);
  ssd_push(display, 0xf1);

  ssd_push(display, SSD1306_SET_VCOMH);
  ssd_push(display, SSD1306_VCOMH_0_77);

  ssd_push(display, SSD1306_ENTIRE_DISPLAY_RESUME);
  ssd_push(display, SSD1306_NORMAL_DISPLAY);
  ssd_push(display, SSD1306_SCROLL_DEACTIVATE);
  ssd_push(display, SSD1306_DISPLAY_ON);

  return ssd_send(display);
}