#include "SSD1306.h"
#include "driver/i2c.h"
#include "portmacro.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"

static char I2C_addr = 0;
static int I2CHandle = NULL;
static uint8_t SSD1306FrameBuffer[SSD1306BUFFERSIZE];

static uint8_t SSD1306CursorX = 0;
static uint8_t SSD1306CursorY = 0;

static uint8_t SSD1306OffsetX = 0;
static uint8_t SSD1306OffsetY = 0;

static uint8_t SSD1306Width = SSD1306_WIDTH;
static uint8_t SSD1306Height = SSD1306_HEIGHT;

static uint8_t SSD1306Changed = 0;


void SSD1306_Init(int i2c_master_port, char address)
{
    I2CHandle = i2c_master_port;
    I2C_addr = address;
    if(!I2CHandle)
        return;
    SSD1306_Reset();

    vTaskDelay(100/portTICK_PERIOD_MS);

    //init display
    SSD1306_SetDisplayOn(0);
    SSD1306_WriteCMD(0x20); //Set Mem  Addressing
    SSD1306_WriteCMD(0x00);

    SSD1306_WriteCMD(0xB0);//Set Page start
    
    SSD1306_WriteCMD(0xC0); //Set COM Output Scan Direction


    SSD1306_WriteCMD(0x00); //---set low column address
    SSD1306_WriteCMD(0x10); //---set high column address

    SSD1306_WriteCMD(0x40); //--set start line address - CHECK

    SSD1306_SetContrast(0xFF);

    SSD1306_WriteCMD(0xA1); //--set segment re-map 0 to 127 - CHECK


    SSD1306_WriteCMD(0xA6); //--set normal color


// Set multiplex ratio.
    SSD1306_WriteCMD(0xA8);

    SSD1306_WriteCMD(0x1F); // Seems to work for 128px high displays too.


    SSD1306_WriteCMD(0xA4); //0xa4,Output follows RAM content;0xa5,Output ignores RAM content

    SSD1306_WriteCMD(0xD3); //-set display offset - CHECK
    SSD1306_WriteCMD(0x00); //-not offset

    SSD1306_WriteCMD(0xD5); //--set display clock divide ratio/oscillator frequency
    SSD1306_WriteCMD(0xF0); //--set divide ratio

    SSD1306_WriteCMD(0xD9); //--set pre-charge period
    SSD1306_WriteCMD(0x22); //

    SSD1306_WriteCMD(0xDA); //--set com pins hardware configuration - CHECK

    SSD1306_WriteCMD(0x02);

    SSD1306_WriteCMD(0xDB); //--set vcomh
    SSD1306_WriteCMD(0x20); //0x20,0.77xVcc

    SSD1306_WriteCMD(0x8D); //--set DC-DC enable
    SSD1306_WriteCMD(0x14); //
    SSD1306_SetDisplayOn(1); //--turn on SSD1306 panel


    //Set buffer to 0;
    for(uint16_t i =0; i < SSD1306BUFFERSIZE; i++)
        SSD1306FrameBuffer[i] = 0;
}

void SSD1306_UpdateScreen()
{
    if(!I2CHandle)
        return;
    if(!SSD1306Changed)
        return;
    SSD1306Changed = 0;

  for(uint8_t i = 0; i < SSD1306_HEIGHT/8; i++) {
        SSD1306_WriteCMD(0xB0 + i); // Set the current RAM page address.
        SSD1306_WriteCMD(0x00);
        SSD1306_WriteCMD(0x10);
        SSD1306_WriteData(&SSD1306FrameBuffer[SSD1306_WIDTH*i],SSD1306_WIDTH);
    }
}

void SSD1306_WriteCMD(unsigned char cmd)
{
    if(!I2CHandle)
        return;
    i2c_master_write_to_device(I2CHandle,I2C_addr,&cmd,1,100/portTICK_PERIOD_MS);
}

void SSD1306_WriteData(unsigned char *data,unsigned char len)
{
    if(!I2CHandle)
        return;
    i2c_master_write_to_device(I2CHandle,I2C_addr,data,len,100/portTICK_PERIOD_MS);
}


void SSD1306_WriteChar(char c)
{
    if(!I2CHandle)
        return;
    //First check if we have enough space to draw the character
    //Font size is 8x8
    //first we check width
    //The display is rotated 90* so we this is weird...
    if((SSD1306CursorX+1)*8 > SSD1306Height-SSD1306OffsetX)
    {
        //We do not fit so jump to the next line and set cursor back to 0
        SSD1306CursorY ++;
        SSD1306CursorX = SSD1306OffsetX;
    }
    //if we do not fit at the bottom jump to top
    if((SSD1306CursorY+1)+8 > SSD1306Width-SSD1306OffsetY)
    {
        SSD1306CursorX = SSD1306OffsetX;
        SSD1306CursorY = SSD1306OffsetY;
    }



    uint16_t offset = (SSD1306CursorY*8)+(SSD1306CursorX*128);
    for(uint16_t i =0 ; i < 8;i++)
    {
        SSD1306FrameBuffer[i+offset] = Font6x8[(c-32)*8+i];
    }
    SSD1306CursorX++;

    SSD1306Changed =1;
}


void SSD1306_WriteString(char *c, char len)
{
    if(!I2CHandle)
        return;
    uint8_t i =0;
    for(i=0;i<len;i++)
    {
        if(c[i]==0)
            break;
        SSD1306_WriteChar(c[i]);
    }
}

void SSD1306_SetCursor(char x,char y)
{
    if(!I2CHandle)
        return;
    SSD1306CursorX = x;
    SSD1306CursorY = y;
}

void SSD1306_SetTextArea(char offsetX, char width, char offsetY, char height)
{
    if(!I2CHandle)
        return;
    SSD1306OffsetX = offsetX;
    SSD1306OffsetY = offsetY;
    SSD1306Width = width;
    SSD1306Height = height;

    SSD1306CursorX = offsetX;
    SSD1306CursorY = offsetY;
}

void SSD1306_SetPixel(char x, char y, char value)
{
    if(!I2CHandle)
        return;
    //This does 8 pixels at once
    SSD1306FrameBuffer[(y)+(x*128)] = value;
    SSD1306Changed =1;
}

//Warning There are NO checks if it fits or if the array is too big
void SSD1306_LoadImage(char StartX, char StartY, char ** data, char width,char height)
{
    if(!I2CHandle)
        return;
    for(uint8_t x =0;x<width;x++)
    {
        for(uint8_t y=0; y<height;y++)
        {
            //We start drawing here
            //This is weird because everything is rotated 90*
            SSD1306_SetPixel(x+StartX,y+StartY,data[x][y]);
        }
    }

}

void SSD1306_Reset()
{
    if(!I2CHandle)
        return;
}

void SSD1306_SetContrast(char contrast)
{
    if(!I2CHandle)
        return;
    SSD1306_WriteCMD(0x81);
    SSD1306_WriteCMD(contrast);
}

void SSD1306_SetDisplayOn(char OnOff)
{
    if(!I2CHandle)
        return;
    if(OnOff)
    {
        SSD1306_WriteCMD(0xAF);
    }
    else
        SSD1306_WriteCMD(0xAE);
}

void SSD1306_Clear()
{
    if(!I2CHandle)
        return;
    for(int i =0; i< SSD1306BUFFERSIZE; i++)
    {
        SSD1306FrameBuffer[i] = 0;
    }
    SSD1306Changed =1;
}

uint8_t Font6x8[] = {
    0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,
0x4,0x4,0x4,0x4,0x4,0x0,0x4,0x0,
0xa,0xa,0xa,0x0,0x0,0x0,0x0,0x0,
0xa,0xa,0x1f,0xa,0x1f,0xa,0xa,0x0,
0x4,0x1e,0x5,0xe,0x14,0xf,0x4,0x0,
0x3,0x13,0x8,0x4,0x2,0x19,0x18,0x0,
0x2,0x5,0x5,0x2,0x15,0x9,0x16,0x0,
0xc,0xc,0x4,0x2,0x0,0x0,0x0,0x0,
0x8,0x4,0x2,0x2,0x2,0x4,0x8,0x0,
0x2,0x4,0x8,0x8,0x8,0x4,0x2,0x0,
0x4,0x15,0xe,0x1f,0xe,0x15,0x4,0x0,
0x0,0x4,0x4,0x1f,0x4,0x4,0x0,0x0,
0x0,0x0,0x0,0x0,0xc,0xc,0x4,0x0,
0x0,0x0,0x0,0x1f,0x0,0x0,0x0,0x0,
0x0,0x0,0x0,0x0,0x0,0xc,0xc,0x0,
0x0,0x10,0x8,0x4,0x2,0x1,0x0,0x0,
0xe,0x11,0x19,0x15,0x13,0x11,0xe,0x0,
0x4,0x6,0x4,0x4,0x4,0x4,0xe,0x0,
0xe,0x11,0x10,0xe,0x1,0x1,0x1f,0x0,
0x1f,0x10,0x8,0xc,0x10,0x11,0xe,0x0,
0x8,0xc,0xa,0x9,0x1f,0x8,0x8,0x0,
0x1f,0x1,0xf,0x10,0x10,0x11,0xe,0x0,
0x1c,0x2,0x1,0xf,0x11,0x11,0xe,0x0,
0x1f,0x10,0x10,0x8,0x4,0x2,0x1,0x0,
0xe,0x11,0x11,0xe,0x11,0x11,0xe,0x0,
0xe,0x11,0x11,0x1e,0x10,0x8,0x7,0x0,
0x0,0x0,0x4,0x0,0x4,0x0,0x0,0x0,
0x0,0x0,0x4,0x0,0x4,0x4,0x2,0x0,
0x10,0x8,0x4,0x2,0x4,0x8,0x10,0x0,
0x0,0x0,0x1f,0x0,0x1f,0x0,0x0,0x0,
0x2,0x4,0x8,0x10,0x8,0x4,0x2,0x0,
0xe,0x11,0x10,0xc,0x4,0x0,0x4,0x0,
0xe,0x11,0x15,0x1d,0xd,0x1,0x1e,0x0,
0x4,0xa,0x11,0x11,0x1f,0x11,0x11,0x0,
0xf,0x11,0x11,0xf,0x11,0x11,0xf,0x0,
0xe,0x11,0x1,0x1,0x1,0x11,0xe,0x0,
0xf,0x11,0x11,0x11,0x11,0x11,0xf,0x0,
0x1f,0x1,0x1,0xf,0x1,0x1,0x1f,0x0,
0x1f,0x1,0x1,0xf,0x1,0x1,0x1,0x0,
0x1e,0x11,0x1,0x1,0x19,0x11,0x1e,0x0,
0x11,0x11,0x11,0x1f,0x11,0x11,0x11,0x0,
0xe,0x4,0x4,0x4,0x4,0x4,0xe,0x0,
0x1c,0x8,0x8,0x8,0x8,0x9,0x6,0x0,
0x11,0x9,0x5,0x3,0x5,0x9,0x11,0x0,
0x1,0x1,0x1,0x1,0x1,0x1,0x1f,0x0,
0x11,0x1b,0x15,0x15,0x15,0x11,0x11,0x0,
0x11,0x11,0x13,0x15,0x19,0x11,0x11,0x0,
0xe,0x11,0x11,0x11,0x11,0x11,0xe,0x0,
0xf,0x11,0x11,0xf,0x1,0x1,0x1,0x0,
0xe,0x11,0x11,0x11,0x15,0x9,0x16,0x0,
0xf,0x11,0x11,0xf,0x5,0x9,0x11,0x0,
0xe,0x11,0x1,0xe,0x10,0x11,0xe,0x0,
0x1f,0x15,0x4,0x4,0x4,0x4,0x4,0x0,
0x11,0x11,0x11,0x11,0x11,0x11,0xe,0x0,
0x11,0x11,0x11,0x11,0x11,0xa,0x4,0x0,
0x11,0x11,0x11,0x15,0x15,0x15,0xa,0x0,
0x11,0x11,0xa,0x4,0xa,0x11,0x11,0x0,
0x11,0x11,0xa,0x4,0x4,0x4,0x4,0x0,
0x1f,0x10,0x8,0xe,0x2,0x1,0x1f,0x0,
0x1e,0x2,0x2,0x2,0x2,0x2,0x1e,0x0,
0x0,0x1,0x2,0x4,0x8,0x10,0x0,0x0,
0x1e,0x10,0x10,0x10,0x10,0x10,0x1e,0x0,
0x4,0xa,0x11,0x0,0x0,0x0,0x0,0x0,
0x0,0x0,0x0,0x0,0x0,0x0,0x1f,0x0,
0x6,0x6,0x4,0x8,0x0,0x0,0x0,0x0,
0x0,0x0,0x6,0x8,0xe,0x9,0x1e,0x0,
0x1,0x1,0xd,0x13,0x11,0x13,0xd,0x0,
0x0,0x0,0xe,0x11,0x1,0x11,0xe,0x0,
0x10,0x10,0x16,0x19,0x11,0x19,0x16,0x0,
0x0,0x0,0xe,0x11,0x1f,0x1,0xe,0x0,
0x8,0x14,0x4,0xe,0x4,0x4,0x4,0x0,
0x0,0x0,0xe,0x19,0x19,0x16,0x10,0x0,
0x1,0x1,0xd,0x13,0x11,0x11,0x11,0x0,
0x4,0x0,0x6,0x4,0x4,0x4,0xe,0x0,
0x8,0x0,0x8,0x8,0x8,0x9,0x6,0x0,
0x1,0x1,0x9,0x5,0x3,0x5,0x9,0x0,
0x6,0x4,0x4,0x4,0x4,0x4,0xe,0x0,
0x0,0x0,0xb,0x15,0x15,0x15,0x15,0x0,
0x0,0x0,0xd,0x13,0x11,0x11,0x11,0x0,
0x0,0x0,0xe,0x11,0x11,0x11,0xe,0x0,
0x0,0x0,0xd,0x13,0x13,0xd,0x1,0x0,
0x0,0x0,0x16,0x19,0x19,0x16,0x10,0x0,
0x0,0x0,0xd,0x13,0x1,0x1,0x1,0x0,
0x0,0x0,0x1e,0x1,0xe,0x10,0xf,0x0,
0x4,0x4,0x1f,0x4,0x4,0x14,0x8,0x0,
0x0,0x0,0x11,0x11,0x11,0x19,0x16,0x0,
0x0,0x0,0x11,0x11,0x11,0xa,0x4,0x0,
0x0,0x0,0x11,0x11,0x15,0x15,0xa,0x0,
0x0,0x0,0x11,0xa,0x4,0xa,0x11,0x0,
0x0,0x0,0x11,0x11,0x1e,0x10,0x11,0x0,
0x0,0x0,0x1f,0x8,0x4,0x2,0x1f,0x0,
0x8,0x4,0x4,0x2,0x4,0x4,0x8,0x0,
0x4,0x4,0x4,0x0,0x4,0x4,0x4,0x0,
0x2,0x4,0x4,0x8,0x4,0x4,0x2,0x0,
0x2,0x15,0x8,0x0,0x0,0x0,0x0,0x0
};