// based on adafruit and sparkfun libraries

#include <string.h> // for memset
#include "ssd1306.h"

unsigned char ssd1306_write = 0b01111000; // i2c address
unsigned char ssd1306_read = 0b01111001; // i2c address
unsigned char ssd1306_buffer[1024]; // 128x32/8. Every bit is a pixel

void ssd1306_setup() {
    // give a little delay for the ssd1306 to power up
    i2c_config_t config;
    config.mode=I2C_MODE_MASTER;
    config.scl_io_num=22;
    config.sda_io_num=21;
    config.master.clk_speed=400000;
    i2c_param_config(I2C_NUM_0,&config);
    i2c_driver_install(I2C_NUM_0,I2C_MODE_MASTER,0,0,0);    
    ssd1306_command(SSD1306_DISPLAYOFF);
    ssd1306_command(SSD1306_SETDISPLAYCLOCKDIV);
    ssd1306_command(0x80);
    ssd1306_command(SSD1306_SETMULTIPLEX);
    ssd1306_command(0x3F); // height-1 = 63
    ssd1306_command(SSD1306_SETDISPLAYOFFSET);
    ssd1306_command(0x0);
    ssd1306_command(SSD1306_SETSTARTLINE);
    ssd1306_command(SSD1306_CHARGEPUMP);
    ssd1306_command(0x14);
    ssd1306_command(SSD1306_MEMORYMODE);
    ssd1306_command(0x00);
    ssd1306_command(SSD1306_SEGREMAP | 0x1);
    ssd1306_command(SSD1306_COMSCANDEC);
    ssd1306_command(SSD1306_SETCOMPINS);
    ssd1306_command(0x02);
    ssd1306_command(SSD1306_SETCONTRAST);
    ssd1306_command(0x1F);
    ssd1306_command(SSD1306_SETPRECHARGE);
    ssd1306_command(0xF1);
    ssd1306_command(SSD1306_SETVCOMDETECT);
    ssd1306_command(0x40);
    ssd1306_command(SSD1306_DISPLAYON);
    ssd1306_clear();
    ssd1306_update();
}

// send a command instruction (not pixel data)
void ssd1306_command(unsigned char c) {
    i2c_cmd_handle_t cmd =i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd,ssd1306_write,1);
    i2c_master_write_byte(cmd, 0x00,1); // bit 7 is 0 for Co bit (data bytes only), bit 6 is 0 for DC (data is a command))
    i2c_master_write_byte(cmd,c,1);
    i2c_master_stop(cmd);
    i2c_master_cmd_begin(I2C_NUM_0,cmd,1000/portTICK_RATE_MS);
}

// update every pixel on the screen
void ssd1306_update() {
    ssd1306_command(SSD1306_PAGEADDR);
    ssd1306_command(0);
    ssd1306_command(0xFF);
    ssd1306_command(SSD1306_COLUMNADDR);
    ssd1306_command(0);
    ssd1306_command(128 - 1); // Width

    unsigned short count = 1024; // WIDTH * ((HEIGHT + 7) / 8)
    unsigned char * ptr = ssd1306_buffer; // first address of the pixel buffer
    i2c_cmd_handle_t cmd =i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd,ssd1306_write,1);
    i2c_master_write_byte(cmd,0x40,1); // send pixel data
    

 //   cmd =i2c_cmd_link_create();
 //   i2c_master_start(cmd);
    while (count--) {
        i2c_master_write_byte(cmd, *ptr++,1);
    }
    i2c_master_stop(cmd);
    i2c_master_cmd_begin(I2C_NUM_0,cmd,1000/portTICK_RATE_MS);
}

// set a pixel value. Call update() to push to the display)
void ssd1306_drawPixel(unsigned char x, unsigned char y, unsigned char color) {
    if ( (x >= 128) || (y >= 64)) {
        return;
    }

    if (color == 1) {
        ssd1306_buffer[x + (y / 8)*128] |= (1 << (y & 7));
    } else {
        ssd1306_buffer[x + (y / 8)*128] &= ~(1 << (y & 7));
    }
}
char row=0; //max 6 
char column=0; //max 20

// zero every pixel value
void ssd1306_clear() {
    memset(ssd1306_buffer, 0, 1024); // make every bit a 0, memset in string.h
    row=0;
    column=0;
}
 

void ssd1306_drawChar(unsigned char x, unsigned char y, unsigned char c){
    c-=0x20;
    for(int i=0;i<5;i++){
        for(int j=0;j<8;j++){
            if((ASCII[c][i])& (1<<j))
                ssd1306_drawPixel(x+i,y+j,1);
            else   
                ssd1306_drawPixel(x+i,y+j,0);

        }
    }

}
void ssd_1306_print(  char* c, uint l){
    for(int i=0;i<l;i++){
        ssd1306_drawChar(column*6,row*9,c[i]);
        column++;
        if(column==22)
            ssd_1306_newLine();
    }
}
void ssd_1306_newLine(){
    if(row<6){
        row++;
    }
    else{
        row=0;
    }
    column=0;
}