#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/pio.h"
#include "hardware/dma.h"
#include "rgb.pio.h"
#include "hsync.pio.h"
#include "vsync.pio.h"

#define RED_PIN 0
#define HSYNC 6
#define VSYNC 7

#define RGB_ACTIVE 127 // 640/5-1
#define H_ACTIVE 655   // 640+16-1
#define V_ACTIVE 479   // 480-1

#define TXCOUNT 61440
uint32_t vga_data_array[TXCOUNT];
uint32_t *address_pointer = &vga_data_array[0];

// # TX_COUNT=640*480/5=61440
// 61440*32=1966080/8=245760=245.76kB
// 264kB-245.76=18.240kB left
// 80*60=4800 bytes = 640*480

void drawPixel(char x, char y, char color)
{
  if (x > 639)
    x = 639;
  if (x < 0)
    x = 0;
  if (y < 0)
    y = 0;
  if (y > 479)
    y = 479;

  int pixel = ((640 * y) + x);
  // Put 5 pixel values into a single 32-bit integer
  vga_data_array[pixel / 5] |= (color << (24 - ((pixel % 5) * 6)));
}

int main() {
  stdio_init_all();

  PIO pio = pio0;

  uint hsync_offset = pio_add_program(pio, &hsync_program);
  uint vsync_offset = pio_add_program(pio, &vsync_program);
  uint rgb_offset = pio_add_program(pio, &rgb_program);

  uint hsync_sm = 0;
  uint vsync_sm = 1;
  uint rgb_sm = 2;

  hsync_program_init(pio, hsync_sm, hsync_offset, HSYNC);
  vsync_program_init(pio, vsync_sm, vsync_offset, VSYNC);
  rgb_program_init(pio, rgb_sm, rgb_offset, RED_PIN);

  int rgb_chan_0 = 0;
  int rgb_chan_1 = 1;

  dma_channel_config c0 = dma_channel_get_default_config(rgb_chan_0); // default configs
  channel_config_set_transfer_data_size(&c0, DMA_SIZE_32);            // 32-bit txfers
  channel_config_set_read_increment(&c0, true);                       // yes read incrementing
  channel_config_set_write_increment(&c0, false);                     // no write incrementing
  channel_config_set_dreq(&c0, DREQ_PIO0_TX2);                        // DREQ_PIO0_TX2 pacing (FIFO)
  channel_config_set_chain_to(&c0, rgb_chan_1);                       // chain to other channel
  
  dma_channel_configure(
    rgb_chan_0,        // Channel to be configured
    &c0,               // The configuration we just created
    &pio->txf[rgb_sm], // write address (RGB PIO TX FIFO)
    &vga_data_array,   // The initial read address (pixel color array)
    TXCOUNT,           // Number of transfers; in this case each is 4 bytes.
    false              // Start immediately.
  );

   // Channel One (reconfigures the first channel)
    dma_channel_config c1 = dma_channel_get_default_config(rgb_chan_1); // default configs
    channel_config_set_transfer_data_size(&c1, DMA_SIZE_32);            // 32-bit txfers
    channel_config_set_read_increment(&c1, false);                      // no read incrementing
    channel_config_set_write_increment(&c1, false);                     // no write incrementing
    channel_config_set_chain_to(&c1, rgb_chan_0);                       // chain to other channel

    dma_channel_configure(
        rgb_chan_1,                        // Channel to be configured
        &c1,                               // The configuration we just created
        &dma_hw->ch[rgb_chan_0].read_addr, // Write address (channel 0 read address)
        &address_pointer,                  // Read address (POINTER TO AN ADDRESS)
        1,                                 // Number of transfers, in this case each is 4 byte
        false                              // Don't start immediately.
    );
  
  pio_sm_put_blocking(pio, hsync_sm, H_ACTIVE);
  pio_sm_put_blocking(pio, vsync_sm, V_ACTIVE);
  pio_sm_put_blocking(pio, rgb_sm, RGB_ACTIVE);
  pio_enable_sm_mask_in_sync(pio, ((1u << hsync_sm) | (1u << vsync_sm) | (1u << rgb_sm)));
  dma_start_channel_mask((1u << rgb_chan_0));

  while (true)
  {
    char index = 0;
    char xcounter = 0;
    char ycounter = 0;
    for (char y = 0; y < 480; y++)
    {
      if (ycounter == 8)
      {
        ycounter = 0;
        index = (index + 1) % 64;
      }
      ycounter += 1;
      for (char x = 0; x < 640; x++)
      {
        if (xcounter == 10)
        {
          xcounter = 0;
          index = (index + 1) % 64;
        }
        xcounter += 1;
        drawPixel(x, y, index);
      }
    }
  }
}