#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_init(){
    gpio_config_t io_conf;
    //disable interrupt
    io_conf.intr_type = GPIO_INTR_DISABLE;
    //set as output mode
    io_conf.mode = GPIO_MODE_OUTPUT;
    //bit mask of the pins that you want to set,e.g.GPIO18/19
    io_conf.pin_bit_mask =
         (1ULL<<GPIO_LAT)|(1ULL<<GPIO_OE)|(1ULL<<GPIO_CLK)
        |(1ULL<<GPIO_R1)|(1ULL<<GPIO_G1)|(1ULL<<GPIO_B1)|(1ULL<<GPIO_R2)|(1ULL<<GPIO_G2)|(1ULL<<GPIO_B2)
        |(1ULL<<GPIO_A)|(1ULL<<GPIO_B)|(1ULL<<GPIO_C)|(1ULL<<GPIO_D)|(1ULL<<GPIO_E);
    //disable pull-down mode
    io_conf.pull_down_en = 0;
    //disable pull-up mode
    io_conf.pull_up_en = 0;
    //configure GPIO with the given settings
    gpio_config(&io_conf);

    gpio_set_level(GPIO_OE, 0);

    write_bits(0,0,0,0,0,0);
    for(uint8_t i=0;i<128;i++){
        clock();
    }
    latch();
    gpio_set_level(GPIO_OE, 1);
    gpio_set_level(GPIO_OE, 0);
}

void display_cycle(void* arg){
    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;
}