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

#define LCD_RST 0

#define LCD_CS  1 // 1
#define LCD_RS  2 // 0
#define LCD_WR  3 // 14
#define LCD_RD  4 // 15

#define LCD_D0  5
#define LCD_D1  6
#define LCD_D2  7
#define LCD_D3  8
#define LCD_D4  9
#define LCD_D5  10
#define LCD_D6  11
#define LCD_D7  12


#define st7789_parallel_wrap_target 0
#define st7789_parallel_wrap 1

static const uint16_t st7789_parallel_program_instructions[] = {
            //     .wrap_target
    0x6008, //  0: out    pins, 8         side 0
    0xb042, //  1: nop                    side 1
            //     .wrap
};
static const struct pio_program st7789_parallel_program = {
    .instructions = st7789_parallel_program_instructions,
    .length = 2,
    .origin = -1,
};

static inline pio_sm_config st7789_parallel_program_get_default_config(uint offset) {
    pio_sm_config c = pio_get_default_sm_config();
    sm_config_set_wrap(&c, offset + st7789_parallel_wrap_target, offset + st7789_parallel_wrap);
    sm_config_set_sideset(&c, 1, false, false);
    return c;
}

uint32_t parallel_sm;
PIO parallel_pio;
uint32_t parallel_offset;
uint32_t parallel_dma;

unsigned char row_scratch_buf[480 * 3];

void pio_init() {
   parallel_pio = pio1;
   parallel_sm = pio_claim_unused_sm(parallel_pio, true);
   parallel_offset = pio_add_program(parallel_pio, &st7789_parallel_program);
   pio_gpio_init(parallel_pio, LCD_WR);
   for(int i = 0; i < 8; i++) {
      pio_gpio_init(parallel_pio, LCD_D0 + i); // NB: must be sequential GPIO numbers starting from D0
   }
   pio_sm_set_consecutive_pindirs(parallel_pio, parallel_sm, LCD_D0, 8, true);
   pio_sm_set_consecutive_pindirs(parallel_pio, parallel_sm, LCD_WR, 1, true);

   pio_sm_config c = st7789_parallel_program_get_default_config(parallel_offset);

   sm_config_set_out_pins(&c, LCD_D0, 8);
   sm_config_set_sideset_pins(&c, LCD_WR);
   sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX);
   sm_config_set_out_shift(&c, false, true, 8);
   sm_config_set_clkdiv(&c, 4);

   pio_sm_init(parallel_pio, parallel_sm, parallel_offset, &c);
   pio_sm_set_enabled(parallel_pio, parallel_sm, true);

   parallel_dma = dma_claim_unused_channel(true);
   dma_channel_config config = dma_channel_get_default_config(parallel_dma);
   channel_config_set_transfer_data_size(&config, DMA_SIZE_8);
   channel_config_set_bswap(&config, false);
   channel_config_set_dreq(&config, pio_get_dreq(parallel_pio, parallel_sm, true));
   dma_channel_configure(parallel_dma, &config, &parallel_pio->txf[parallel_sm], NULL, 0, false);
}

void init_pins() {
   // gpio_init sets pio inputs
   gpio_init(LCD_RST);
   gpio_init(LCD_CS);
   gpio_init(LCD_RS);
   gpio_init(LCD_RD);

   gpio_set_dir(LCD_RST, GPIO_OUT);
   gpio_set_dir(LCD_CS,  GPIO_OUT);
   gpio_set_dir(LCD_RS,  GPIO_OUT);
   gpio_set_dir(LCD_RD,  GPIO_OUT);

   gpio_put(LCD_RST, 1);
   gpio_put(LCD_CS,  1);
   gpio_put(LCD_RS,  1);
   gpio_put(LCD_RD,  1);

   pio_init();
}

void lcd_put_data_bulk(unsigned char *data, int len) {
   while (dma_channel_is_busy(parallel_dma))
      ;
   dma_channel_set_trans_count(parallel_dma, len, false);
   dma_channel_set_read_addr(parallel_dma, data, true);
}

void lcd_put_data(unsigned char d) {
   lcd_put_data_bulk(&d, 1);
}

void lcd_write_cmd(unsigned char cmd) {
   gpio_put(LCD_RS, 0);
   lcd_put_data(cmd);
}

void lcd_write_data_bulk(unsigned char *data, int len) {
   gpio_put(LCD_RS, 1);
   lcd_put_data_bulk(data, len);
}

void lcd_write_data(unsigned char d) {
   gpio_put(LCD_RS, 1);
   lcd_put_data(d);
}

void lcd_init() {
   init_pins();

   gpio_put(LCD_CS, 1);
   gpio_put(LCD_CS, 0);

   // Reset LCD
   gpio_put(LCD_RST, 1);
   sleep_ms(100);
   gpio_put(LCD_RST, 0);
   sleep_ms(100);
   gpio_put(LCD_RST, 1);
   sleep_ms(120);

    // sleep out
    lcd_write_cmd(0x11);
    sleep_ms(100);
    // display on
    lcd_write_cmd(0x29);
    sleep_ms(100);
}

void fill_screen(unsigned char r, unsigned char g, unsigned char b) {
    /* for (int i = 0; i < 480; i++) { */
    /*    row_scratch_buf[(3*i) + 0] = b; */
    /*    row_scratch_buf[(3*i) + 1] = g; */
    /*    row_scratch_buf[(3*i) + 2] = r; */
    /* } */

    lcd_write_cmd(0x2c); // write mem
    for (int i = 0; i < 320*480; i++) {
       lcd_write_data(b); // B
       lcd_write_data(g); // G
       lcd_write_data(r); // R
    }

    /* for (int i = 0; i < 320; i++) { */
    /*    lcd_write_data_bulk(row_scratch_buf, 480*3); */
    /* } */
}


int main() {
    stdio_init_all();

    lcd_init();

    while (true) {
       fill_screen(0xff, 0, 0);
       fill_screen(0, 0xff, 0);
       fill_screen(0, 0, 0xff);
    }

    return 0;
}