#include"ssd1306.h"

static uint8_t SSD1306_Buffer[SSD1306_HEIGHT * SSD1306_WIDTH / 8];

// Een scherm-object om lokaal in te werken
static SSD1306_t SSD1306;

void startscrollright(uint8_t start, uint8_t stop){
  ssd1306_WriteCommand(SSD1306_RIGHT_HORIZONTAL_SCROLL);
  ssd1306_WriteCommand(0X00);
  ssd1306_WriteCommand(start);
  ssd1306_WriteCommand(0X00);
  ssd1306_WriteCommand(stop);
  ssd1306_WriteCommand(0X00);
  ssd1306_WriteCommand(0XFF);
  ssd1306_WriteCommand(SSD1306_ACTIVATE_SCROLL);
}


void drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1)
{
    int16_t steep = abs(y1 - y0) > abs(x1 - x0);
    if (steep) {
        _swap_int16_t(x0, y0);
        _swap_int16_t(x1, y1);
    }

    if (x0 > x1) {
        _swap_int16_t(x0, x1);
        _swap_int16_t(y0, y1);
    }

    int16_t dx, dy;
    dx = x1 - x0;
    dy = abs(y1 - y0);

    int16_t err = dx / 2;
    int16_t ystep;

    if (y0 < y1) {
        ystep = 1;
    } else {
        ystep = -1;
    }

    for (; x0<=x1; x0++) {
        if (steep) {
            ssd1306_DrawPixel(y0, x0, White);
        } else {
            ssd1306_DrawPixel(x0, y0, White);
        }
        err -= dy;
        if (err < 0) {
            y0 += ystep;
            err += dx;
        }
    }
}

static int anim = 1;
static int x = 0;

void ssd1306_LoadImage(void) {
   //memcpy(SSD1306_Buffer, image, 1024);
   fastlz_decompress(image, 1024, SSD1306_Buffer, 1024); 
}

void ssd1306_DrawSprite(SpriteDef *sprite, int8_t x, int8_t y) {
    int spriteAddr = 0;
    int currX, currY = 0;

    for(int bank = y / 8; bank < sprite->height / 8; bank++) {
        int start = (bank * sprite->width) + (bank * sprite->height) + (x);
        currX = x;
        int end = start + sprite->width;
        for(int addr = start; addr < end; addr++) {

            if (currX > 0 && currY > 0 && currX < SSD1306_WIDTH && currY < SSD1306_HEIGHT) {
                SSD1306_Buffer[addr] = sprite->buffer[spriteAddr];
            }
            
            spriteAddr++;
            currX++;
        }
        currY += 8;
    }
}

void ssd1306_Anim(void) {
    SpriteDef *sprite;

    switch(anim) {
        case 0:
            sprite = &sprite_0;
            break;
        case 1:
            sprite = &sprite_1;
            break;
        case 2:
            sprite = &sprite_2;
            break;
        case 3:
            sprite = &sprite_3;
            break;
        case 4:
            sprite = &sprite_4;
            break;
    }

    ssd1306_DrawSprite(sprite, x, 0);

    anim++;
    x += 15;

    if (anim == 5) {
        anim = 1;
    }

    if (x > SSD1306_WIDTH) {
        x = -64;
    }
}

/**
 * [ssd1306_ScrollUp scrolls the whole LCD buffer up]
 * @param pixels [How many pixels to scroll up]
 * @param delay  [Delay between refresh]
 */
//void ssd1306_ScrollUp(unsigned int pixels, unsigned int delay, )
void ssd1306_ScrollUp(unsigned int pixels, unsigned int delay, UART_HandleTypeDef uart)
{
    static uint32_t end_addr = SSD1306_WIDTH * 8;
    char lost_bit = 0;
    //char buffer[64];

    for(uint8_t times = 0; times < pixels; times++) {
        for(uint32_t addr = 0; addr < end_addr; addr++) {
            //char binary[33];
            //itoa(SSD1306_Buffer[addr], binary, 2);
            //sprintf(buffer, "addr: %x : %s\n\r", addr, binary);
            //HAL_UART_Transmit(&uart, (uint8_t*)buffer, strlen(buffer), 0xFFFF);


            // In the first bank we dont need to keep the lost bit
            if (addr > 0x7f) {
                lost_bit = SSD1306_Buffer[addr] & 0x01;
            }

            // Shift one pixel "up"
            SSD1306_Buffer[addr] >>= 1;

            // In the first bank we dont need to do this
            if (addr > 0x7f) {
                // Move the saved bit from the LSB to the MSB
                lost_bit <<= 7;
                // Add the saved bit to the bank above it
                SSD1306_Buffer[addr - SSD1306_WIDTH] |= lost_bit;
            }
        }
        ssd1306_UpdateScreen();
        HAL_Delay(delay);
    }
}

//
//  Een byte sturen naar het commando register
//  Kan niet gebruikt worden buiten deze file
//
void ssd1306_WriteCommand(uint8_t command)
{
    HAL_I2C_Mem_Write(&hi2c1, SSD1306_I2C_ADDR, 0x00,1, &command, 1, 10);
}


//
//  Het scherm initialiseren voor gebruik
//
uint8_t ssd1306_Init(void)
{   
    // Even wachten zodat het scherm zeker opgestart is
    HAL_Delay(100);
    
    ssd1306_WriteCommand(SSD1306_DISPLAYOFF);                    // 0xAE
    ssd1306_WriteCommand(SSD1306_SETDISPLAYCLOCKDIV);            // 0xD5
    ssd1306_WriteCommand(0x80);                                  // the suggested ratio 0x80

    ssd1306_WriteCommand(SSD1306_SETMULTIPLEX);                  // 0xA8
    ssd1306_WriteCommand(SSD1306_HEIGHT - 1);

    ssd1306_WriteCommand(SSD1306_SETDISPLAYOFFSET);              // 0xD3
    ssd1306_WriteCommand(0x0);                                   // no offset
    ssd1306_WriteCommand(SSD1306_SETSTARTLINE | 0x0);            // line #0
    ssd1306_WriteCommand(SSD1306_CHARGEPUMP);                    // 0x8D

    ssd1306_WriteCommand(0x14);

    ssd1306_WriteCommand(SSD1306_MEMORYMODE);                    // 0x20
    ssd1306_WriteCommand(0x00);                                  // 0x0 act like ks0108
    ssd1306_WriteCommand(SSD1306_SEGREMAP | 0x1);
    ssd1306_WriteCommand(SSD1306_COMSCANDEC);

    ssd1306_WriteCommand(SSD1306_SETCOMPINS);                    // 0xDA
    ssd1306_WriteCommand(0x12);
    ssd1306_WriteCommand(SSD1306_SETCONTRAST);                   // 0x81
    ssd1306_WriteCommand(0xCF);

    ssd1306_WriteCommand(SSD1306_SETPRECHARGE);
    ssd1306_WriteCommand(0xF1);

    ssd1306_WriteCommand(SSD1306_SETVCOMDETECT);                 // 0xDB
    ssd1306_WriteCommand(0x40);
    ssd1306_WriteCommand(SSD1306_DISPLAYALLON_RESUME);           // 0xA4
    ssd1306_WriteCommand(SSD1306_NORMALDISPLAY);                 // 0xA6

    ssd1306_WriteCommand(SSD1306_DEACTIVATE_SCROLL);

    ssd1306_WriteCommand(SSD1306_DISPLAYON);//--turn on oled panel

    /* Clearen scherm */
    ssd1306_Fill(Black);
    
    /* Update screen */
    ssd1306_UpdateScreen();
    
    /* Set default values */
    SSD1306.CurrentX = 0;
    SSD1306.CurrentY = 0;
    
    /* Initialized OK */
    SSD1306.Initialized = 1;
    
    /* Return OK */
    return 1;
}

//
//  We zetten de hele buffer op een bepaalde kleur
//  color   => de kleur waarin alles moet
//
void ssd1306_Fill(SSD1306_COLOR color) 
{
    /* Set memory */
    uint32_t i;

    for(i = 0; i < sizeof(SSD1306_Buffer); i++)
    {
        SSD1306_Buffer[i] = (color == Black) ? 0x00 : 0xFF;
    }
}

//
//  Alle weizigingen in de buffer naar het scherm sturen
//
void ssd1306_UpdateScreen(void) 
{
    uint8_t i;
    
    for (i = 0; i < 8; i++) {
        ssd1306_WriteCommand(0xB0 + i);
        ssd1306_WriteCommand(0x00);
        ssd1306_WriteCommand(0x10);

        // We schrijven alles map per map weg
        HAL_I2C_Mem_Write(&hi2c1,SSD1306_I2C_ADDR,0x40,1,&SSD1306_Buffer[SSD1306_WIDTH * i],SSD1306_WIDTH,100);
    }
}

//
//  1 pixel op het scherm tekenen
//  X => X coordinaat
//  Y => Y coordinaat
//  color => kleur die pixel moet krijgen
//
void ssd1306_DrawPixel(uint8_t x, uint8_t y, SSD1306_COLOR color)
{
    if (x >= SSD1306_WIDTH || y >= SSD1306_HEIGHT) 
    {
        // We gaan niet buiten het scherm schrijven
        return;
    }
    
    // Kijken of de pixel geinverteerd moet worden
    if (SSD1306.Inverted) 
    {
        color = (SSD1306_COLOR)!color;
    }
    
    // We zetten de juiste kleur voor de pixel
    if (color == White)
    {
        SSD1306_Buffer[x + (y / 8) * SSD1306_WIDTH] |= 1 << (y % 8);
    } 
    else 
    {
        SSD1306_Buffer[x + (y / 8) * SSD1306_WIDTH] &= ~(1 << (y % 8));
    }
}

//
//  We willen 1 char naar het scherm sturen
//  ch      => char om weg te schrijven
//  Font    => Font waarmee we gaan schrijven
//  color   => Black or White
//
char ssd1306_WriteChar(char ch, FontDef Font, SSD1306_COLOR color)
{
    uint32_t i, b, j;
    
    // Kijken of er nog plaats is op deze lijn
    if (SSD1306_WIDTH <= (SSD1306.CurrentX + Font.FontWidth) ||
        SSD1306_HEIGHT <= (SSD1306.CurrentY + Font.FontHeight))
    {
        // Er is geen plaats meer
        return 0;
    }
    
    // We gaan door het font
    for (i = 0; i < Font.FontHeight; i++)
    {
        b = Font.data[(ch - 32) * Font.FontHeight + i];
        for (j = 0; j < Font.FontWidth; j++)
        {
            if ((b << j) & 0x8000) 
            {
                ssd1306_DrawPixel(SSD1306.CurrentX + j, (SSD1306.CurrentY + i), (SSD1306_COLOR) color);
            } 
            else 
            {
                ssd1306_DrawPixel(SSD1306.CurrentX + j, (SSD1306.CurrentY + i), (SSD1306_COLOR)!color);
            }
        }
    }
    
    // De huidige positie is nu verplaatst
    SSD1306.CurrentX += Font.FontWidth;
    
    // We geven het geschreven char terug voor validatie
    return ch;
}

//
//  Functie voor het wegschrijven van een hele string
//  str => string om op het scherm te schrijven
//  Font => Het font dat gebruikt moet worden
//  color => Black or White
//
char ssd1306_WriteString(char* str, FontDef Font, SSD1306_COLOR color)
{
    // We schrijven alle char tot een nulbyte
    while (*str) 
    {
        if (ssd1306_WriteChar(*str, Font, color) != *str)
        {
            // Het karakter is niet juist weggeschreven
            return *str;
        }
        
        // Volgende char
        str++;
    }
    
    // Alles gelukt, we sturen dus 0 terug
    return *str;
}

//
//  Zet de cursor op een coordinaat
//
void ssd1306_SetCursor(uint8_t x, uint8_t y) 
{
    /* Set write pointers */
    SSD1306.CurrentX = x;
    SSD1306.CurrentY = y;
}