//
// Created by MightyPork on 2022/12/28.
//

#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "pico/binary_info.h"

#include "hardware/spi.h"

#include "framebuffer.h"

static const uint DC_PIN = 20;
static const uint RST_PIN = 21;

#define SSD1309_HEIGHT 64

/* Picotool info */
// Make the SPI pins available to picotool
bi_decl(bi_2pins_with_func(PICO_DEFAULT_SPI_TX_PIN, PICO_DEFAULT_SPI_SCK_PIN, GPIO_FUNC_SPI))
bi_decl(bi_1pin_with_name(PICO_DEFAULT_SPI_CSN_PIN, "SPI CS"))
bi_decl(bi_1pin_with_name(DC_PIN, "OLED DC"))
bi_decl(bi_1pin_with_name(RST_PIN, "OLED RST"))

static inline 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 inline 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 inline void dc_command() {
    gpio_put(DC_PIN, 0);
}

static inline void dc_data() {
    gpio_put(DC_PIN, 1);
}

void oled_command16(uint8_t cmd, uint8_t arg) {
    uint8_t buf[2];
    buf[0] = cmd;
    buf[1] = arg;
    dc_command();
    cs_select();
    spi_write_blocking(spi_default, buf, 2);
    cs_deselect();
}

void oled_command(uint8_t cmd) {
    dc_command();
    cs_select();
    spi_write_blocking(spi_default, &cmd, 1);
    cs_deselect();
}

void oled_reset() {
    // Issue a display reset
    gpio_put(RST_PIN, 0);
    sleep_ms(1);
    gpio_put(RST_PIN, 1);
    sleep_ms(1);
}

void oled_invert(bool invert) {
    if (invert) {
        oled_command(0xA6);
    } else {
        oled_command(0xA7);
    }
}

void oled_init() {
    // This example will use SPI0 at 10MHz.
    spi_init(spi_default, 10 * 1000 * 1000);
    gpio_set_function(PICO_DEFAULT_SPI_SCK_PIN, GPIO_FUNC_SPI);
    gpio_set_function(PICO_DEFAULT_SPI_TX_PIN, GPIO_FUNC_SPI);

    // Chip select is active-low, so we'll initialise it to a driven-high state
    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(DC_PIN);
    gpio_set_dir(DC_PIN, GPIO_OUT);

    gpio_init(RST_PIN);
    gpio_set_dir(RST_PIN, GPIO_OUT);

    oled_reset();

    // simplified display init

    oled_command(0xAE); /* Display off */
    //oled_command16(0xD5, 0xF0); // set clock speed

    oled_command16(0x20, 0b00); // page addressing mode: 00 - horizontal, 01 vertical, 10 page

    oled_command16(0x81, 0xFF); // contrast

    oled_command(0xAF); /*--turn on SSD1309 panel */

#if 0

    /* Init OLED */
    oled_command(0xAE); /* Display off */

    // Configure the display - only setting things that differ from the default:


    oled_command16(0x20, 0b10); /* Set Memory Addressing Mode */
    /* 00,Horizontal Addressing Mode; 01,Vertical Addressing Mode; */
    /* 10,Page Addressing Mode (RESET); 11,Invalid */

    oled_command(0xB0); /*Set Page Start Address for Page Addressing Mode, B0-B7 */

#ifdef SSD1309_MIRROR_VERT
    oled_command(0xC1);
#else
    oled_command(0xC0);
#endif

    // lower nibble is value
    oled_command(0x00); /*---set low column address 00~0F  */

    oled_command(0x10); /*---set high column address 10~1F */

    oled_command(0x40); /*--set start line address - 40~7F */

    oled_command16(0x81, 0x7F); /*--set contrast control register - ? 0xFF */

#ifdef SSD1309_MIRROR_HORIZ
    oled_command(0xA1);
#else
    oled_command(0xA0);
#endif

#ifdef SSD1309_INVERSE_COLOR
    oled_command(0xA7); /*--set inverse color */
#else
    oled_command(0xA6); /*--set normal color */
#endif

    /*--set multiplex ratio(1 to 64) */
#if (SSD1309_HEIGHT == 32)
    oled_command16(0xA8, 0x1F);
#elif (SSD1309_HEIGHT == 64)
    oled_command16(0xA8, 0x3F);
#else
#error "bad height"
#endif

    oled_command(0xA4); /* 0xA4, Output follows RAM content;0xa5,Output ignores RAM content */

    oled_command16(0xD3, 0x00); /*-set display offset */

    oled_command16(0xD5, 0xF0); /*--set display clock divide ratio/oscillator frequency */

    oled_command16(0xD9, 0x22); /*--set pre-charge period */

    /*--set com pins hardware configuration */
#if (SSD1309_HEIGHT == 32)
    oled_command16(0xDA, 0x02);
#elif (SSD1309_HEIGHT == 64)
    oled_command16(0xDA, 0x12);
#else
#error "bad height"
#endif

    oled_command16(0xDB, 0x34); /*--set vcomh */

    // ??? not listed in the datasheet
//    oled_command(0x8D); /*--set DC-DC enable */
//    oled_command(0x14); /*                   */

    oled_command(0xAF); /*--turn on SSD1309 panel */


    oled_command(0xA5); // entire display on - for test

#endif
}

void oled_data(uint8_t *data, size_t len) {
    dc_data();
    cs_select();
    spi_write_blocking(spi_default, data, len);
    cs_deselect();
}

void fb_blit() {
    oled_data(fb, FB_LEN);
}