#include "driver/gpio.h"
#include "display.h"

uint8_t fb[64][128][3];

void write_bits(uint8_t r1, uint8_t g1, uint8_t b1, uint8_t r2, uint8_t g2, uint8_t b2){
    gpio_set_level(GPIO_R1, r1);  
    gpio_set_level(GPIO_G1, g1);
    gpio_set_level(GPIO_B1, b1);
    gpio_set_level(GPIO_R2, r2);
    gpio_set_level(GPIO_G2, g2);
    gpio_set_level(GPIO_B2, b2);
}

void write_address(uint8_t addr){
    gpio_set_level(GPIO_A, addr&0x01);
    gpio_set_level(GPIO_B, (addr&0x02)>>1);
    gpio_set_level(GPIO_C, (addr&0x04)>>2);
    gpio_set_level(GPIO_D, (addr&0x08)>>3);
    gpio_set_level(GPIO_E, (addr&0x10)>>4);
}

void latch(){
    gpio_set_level(GPIO_LAT, 0);
    gpio_set_level(GPIO_LAT, 1);
}

void clock(){
    gpio_set_level(GPIO_CLK, 1);
    gpio_set_level(GPIO_CLK, 0);
}

void display_cycle(){
    printf("test");
    static uint8_t line = 0;

    for(uint8_t i=0;i<128;i++){
        write_bits(fb[line][i][0],fb[line][i][1],fb[line][i][2],fb[32+line][i][0],fb[32+line][i][1],fb[32+line][i][2]);
        clock();
    }
    gpio_set_level(GPIO_OE, 1);
    latch();
    write_address(line);
    gpio_set_level(GPIO_OE, 0);
    line = (line + 1)%32;
}