#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 uint8_t anim = 1;
static int16_t x = -64;

/**
 * [ssd1306_Decompress description]
 * @param input       [description]
 * @param output      [description]
 * @param output_size [description]
 */
void ssd1306_Decompress(uint8_t *input, uint8_t *output, uint16_t output_size) {
    z_stream infstream;

    infstream.zalloc = Z_NULL;
    infstream.zfree = Z_NULL;
    infstream.opaque = Z_NULL;
    infstream.avail_in = 0;
    infstream.next_in = Z_NULL;

    infstream.next_in = (Bytef *)input;
    infstream.avail_in = output_size;
    infstream.avail_out = (uInt)output_size;
    infstream.next_out = (Bytef *)output;

    inflateInit(&infstream);
    inflate(&infstream, Z_FINISH);
    inflateEnd(&infstream);
}

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

void ssd1306_DrawSprite(SpriteDef sprite, int16_t x, int16_t y) {
    uint16_t spriteAddr = 0;
    int16_t currX, currY = y;

    for(int16_t bank = currY / 8; bank < sprite.height / 8; bank++) {
        currX = x;
        int16_t start = (bank * sprite.width) + (bank * sprite.height) + x;
        int16_t end = start + sprite.width;

        for(int16_t addr = start; addr < end; addr++) {
            if (spriteAddr < (sprite.height * sprite.width / 8) ) {
                for(uint8_t bit = 0; bit < 8; bit++) {
                    if ((sprite.buffer[spriteAddr] >> bit) & 0x01) {
                        uint8_t newY = bit + currY;
                        if (currX >= 0 && newY >= 0 && currX < SSD1306_WIDTH && newY < SSD1306_HEIGHT) {
                            ssd1306_DrawPixel(currX, newY, White);
                        }
                    }
                }
            }

            spriteAddr++;
            currX++;
        }
        currY += 8;
    }
}

void ssd1306_Anim(void) {
    SpriteDef dstSprite;
    uint16_t dstSize;

    dstSize = animation.height * animation.width / 8;
    dstSprite.width = animation.width;
    dstSprite.height = animation.height;
    dstSprite.buffer = malloc(sizeof(uint8_t) * (dstSize));
    ssd1306_Decompress(animation.buffer[anim], dstSprite.buffer, dstSize);

    ssd1306_DrawSprite(dstSprite, x, 0);

    free(dstSprite.buffer);

    anim++;
    x++;

    if (anim == animation.frames) {
        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(uint8_t pixels, uint8_t delay)
{
    uint32_t end_addr = SSD1306_WIDTH * 8;
    char lost_bit = 0;

    for(uint8_t times = 0; times < pixels; times++) {
        for(uint16_t addr = 0; addr < end_addr; addr++) {
            // 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;
}