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

#include <stdio.h>
#include <string.h>

#include "ufb/framebuffer.h"
#include "gpio.h"
#include "spi.h"

#define OLED_HSPI hspi2

#define SSD1309_HEIGHT 64

static inline void cs_select()
{
    asm volatile("nop \n nop \n nop");
    HAL_GPIO_WritePin(OLED_CS_GPIO_Port, OLED_CS_Pin, 0); // Active low
    asm volatile("nop \n nop \n nop");
}

static inline void cs_deselect()
{
    asm volatile("nop \n nop \n nop");
    HAL_GPIO_WritePin(OLED_CS_GPIO_Port, OLED_CS_Pin, 1);
    asm volatile("nop \n nop \n nop");
}

static inline void dc_command()
{
    HAL_GPIO_WritePin(OLED_DC_GPIO_Port, OLED_DC_Pin, 0);
}

static inline void dc_data()
{
    HAL_GPIO_WritePin(OLED_DC_GPIO_Port, OLED_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();
    HAL_SPI_Transmit(&OLED_HSPI, buf, 2, 1000);
    cs_deselect();
}

void oled_command(uint8_t cmd)
{
    dc_command();
    cs_select();
    HAL_SPI_Transmit(&OLED_HSPI, &cmd, 1, 1000);
    cs_deselect();
}

void oled_reset()
{
    // Issue a display reset
    HAL_GPIO_WritePin(OLED_RST_GPIO_Port, OLED_RST_Pin, 0);
    HAL_Delay(1);
    HAL_GPIO_WritePin(OLED_RST_GPIO_Port, OLED_RST_Pin, 1);
    HAL_Delay(1);
}

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

void oled_init()
{
    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
}

static void transpose8_and_reverse(const uint8_t *a, uint8_t *b)
{
    uint32_t x, y, t;

    // Load the array and pack it into x and y.

    x = (a[0] << 24) | (a[1] << 16) | (a[2] << 8) | a[3];
    y = (a[4] << 24) | (a[5] << 16) | (a[6] << 8) | a[7];

    t = (x ^ (x >> 7)) & 0x00AA00AA;
    x = x ^ t ^ (t << 7);
    t = (y ^ (y >> 7)) & 0x00AA00AA;
    y = y ^ t ^ (t << 7);

    t = (x ^ (x >> 14)) & 0x0000CCCC;
    x = x ^ t ^ (t << 14);
    t = (y ^ (y >> 14)) & 0x0000CCCC;
    y = y ^ t ^ (t << 14);

    t = (x & 0xF0F0F0F0) | ((y >> 4) & 0x0F0F0F0F);
    y = ((x << 4) & 0xF0F0F0F0) | (y & 0x0F0F0F0F);
    x = t;

//    a[0] = x >> 24;
//    a[1] = x >> 16;
//    a[2] = x >> 8;
//    a[3] = x;
//    a[4] = y >> 24;
//    a[5] = y >> 16;
//    a[6] = y >> 8;
//    a[7] = y;

    b[7] = x >> 24;
    b[6] = x >> 16;
    b[5] = x >> 8;
    b[4] = x;
    b[3] = y >> 24;
    b[2] = y >> 16;
    b[1] = y >> 8;
    b[0] = y;
}

void oled_data(uint8_t *data, size_t len)
{
    dc_data();
    cs_select();
    HAL_SPI_Transmit(&OLED_HSPI, data, len, 1000);
    cs_deselect();
}

void fb_blit()
{
#if 0
    oled_data(fb, FB_LEN);
#else
    dc_data();
    cs_select();

    // 90deg
    uint8_t buf[8];
    for (int j = FBW / 8 - 1; j >= 0; j--) {
        for (int i = 0; i < FBH / 8; i++) {
            // i, j are "square coords" (8x8)
            transpose8_and_reverse(&fb[i * FBW + j * 8], buf);
            HAL_SPI_Transmit(&OLED_HSPI, buf, 8, 100);
        }
    }

    cs_deselect();

#endif
}