#include "DFRobot_SSD1306.h"
#include "DFRobot_ASCIIfont.h"

DFRobot_SSD1306::DFRobot_SSD1306(void) {
}


int16_t DFRobot_SSD1306::begin(uint8_t I2CAddr_)
{
    I2CAddr = I2CAddr_;
    textColor = 1;
    _lineWidth = 1;
    Wire.begin();
    //config
    writeCmd(SSD1306_DISPLAYOFF);
    writeCmd(SSD1306_SETDISPLAYCLOCKDIV);
    writeCmd(0xF0); // Increase speed of the display max ~96Hz
    writeCmd(SSD1306_SETMULTIPLEX);
    writeCmd(0x3F);
    writeCmd(SSD1306_SETDISPLAYOFFSET);
    writeCmd(0x00);
    writeCmd(SSD1306_SETSTARTLINE);
    writeCmd(SSD1306_CHARGEPUMP);
    writeCmd(0x14);
    writeCmd(SSD1306_MEMORYMODE);
    writeCmd(0x00);
    writeCmd(SSD1306_SEGREMAP);
    writeCmd(SSD1306_COMSCANINC);
    writeCmd(SSD1306_SETCOMPINS);
    writeCmd(0x12);
    writeCmd(SSD1306_SETCONTRAST);
    writeCmd(0xCF);
    writeCmd(SSD1306_SETPRECHARGE);
    writeCmd(0xF1);
    writeCmd(SSD1306_DISPLAYALLON_RESUME);
    writeCmd(SSD1306_NORMALDISPLAY);
    writeCmd(0x2e);                        // stop scroll
    writeCmd(SSD1306_DISPLAYON);
    setRotaion(eROTATION_180);
    fillScreen(0);
    setTextColor(1);
    return BEGIN_WAR_NOTEST;
}

int16_t DFRobot_SSD1306::setRotaion(eROTATION eRotation)
{
    fillScreen(0);
    switch(eRotation) {
        case eROTATION_0:   writeCmd(0xa0); writeCmd(0xc0); break;
        case eROTATION_180: writeCmd(0xa1); writeCmd(0xc8); break;
        default: return DISPLAY_ERR_NOTSUPPORT;
    }
    return DISPLAY_ERR_OK;
}

void DFRobot_SSD1306::fillScreen(uint16_t color)
{
#if defined(ESP_PLATFORM) || defined(NRF5)
    if(color)    memset(SSD1306_FrameBuffer, 0xff, 1024);
    else     memset(SSD1306_FrameBuffer, 0, 1024);

    byte i,j;
    for (i = 0; i < 8; i++)
    {
#if defined(ESP_PLATFORM)
        // send a bunch of data in one xmission
        writeCmd(0xB0 + i);//set page address
        writeCmd((2+0) & 0xf);//set lower column address
        writeCmd(0x10 | ((2+0) >> 4));//set higher column address
#else defined(NRF5)
        writeCmd(SSD1306_COLUMNADDR);
        writeCmd(0);
        writeCmd(127);
        writeCmd(SSD1306_PAGEADDR);
        writeCmd(i);
        writeCmd(i+1);
#endif
        for( j = 0; j < 8; j++)
        {
            writeDatBytes(SSD1306_FrameBuffer + i*128 + j*16, 16);
        }
    }
#else
    uint8_t    i = 0, j = 0;
    uint8_t    buf[16] = {0};
    if(color) {
        color = 0xff;
        for(i = 0; i < 16; i ++) {
            buf[i] = 0xff;
        }
    }

    writeCmd(SSD1306_COLUMNADDR);
    writeCmd(0x0);
    writeCmd(0x7f);

    writeCmd(SSD1306_PAGEADDR);
    writeCmd(0x0);
    writeCmd(0x7);

    for(i = 0; i < 128 * 64 / 8 / 16; i ++) {
        writeDatBytes(buf, 16);
    }
#endif
}

void DFRobot_SSD1306::showMatrix(uint8_t x, uint8_t y, int width, int height, const uint8_t *pBuf, bool back)
{
    if(!pBuf) return;
    if(x>127 || y>63) return;
    if(height%8 !=0) return;

    int i,j,k;
    uint8_t _x;
    uint8_t _y;
    int widthSize = width/8;
    int heightSize = height/8;

#if defined(ESP_PLATFORM) || defined(NRF5)
    _x = x;
    _y = y;
#else
    _x = 0;
    _y = 0;
    int writeWidth = width;
    int matrixSize = width * height / 8;
    uint8_t matrixBuffer[matrixSize];
    memset(matrixBuffer, 0, matrixSize);
    int16_t bufferAddr = 0;
#endif

    for(i=0; i<height; i++)
    {
        if(_y > 63) break;
        for(j=0; j<widthSize; j++)
        {
            uint8_t data = *pBuf;
            for(k=0; k<8; k++)
            {
                if(_x > 127) break;
#if defined(ESP_PLATFORM) || defined(NRF5)
                if(data & 0x80)    writeBufferPixel(_x, _y, 1);
                else {
                    if(back) writeBufferPixel(_x, _y, 0);
                }
#else
                bufferAddr = _x + _y / 8 * width;
                if(data & 0x80)    matrixBuffer[bufferAddr] |= (0x01 << (_y % 8));
                else {
                    if(back) matrixBuffer[bufferAddr] &= ~ (0x01 << (_y % 8));
                }
#endif
                data <<= 1;
                _x++;
            }
            pBuf++;
        }
#if defined(ESP_PLATFORM) || defined(NRF5)
        _x = x;
#else
        _x = 0;
#endif
        _y++;
    }

#if defined(ESP_PLATFORM) || defined(NRF5)
    regionalRefresh(x,y,width,height);
#else
    _x = x, _y = y;
    for(i=0; i<heightSize; i++)
    {
        writeCmd(SSD1306_COLUMNADDR);
        writeCmd(_x);
        writeCmd(_x + width -1);
        writeCmd(SSD1306_PAGEADDR);
        writeCmd((_y+i*8) / 8);
        writeCmd((_y+i*8 + 8) / 8);
        writeDatBytes(matrixBuffer + i*width,writeWidth);
    }
#endif
}

void DFRobot_SSD1306::displayON(void)
{
    writeCmd(0xaf);
}

void DFRobot_SSD1306::displayOFF(void)
{
    writeCmd(0xae);
}

void DFRobot_SSD1306::setTextColor(uint16_t color)
{
    textColor = color;
}

void DFRobot_SSD1306::setCursor(int16_t x, int16_t y)
{
#if defined(ESP_PLATFORM) || defined(NRF5)
    if(x<0) x=0;
    if(x>127) x=127;
    if(y<0) y=0;
    if(y>63) y=63;
    printfX = x;
    printfY = y;
#else
    if(x > 127) {
        printfX = 127;
    } else {
        printfX = x;
    }
    y = 16 * y;
    if(y>=0 && y<16)
        printfY = 0;
    else if(y>=16 && y<32)
        printfY = 16;
    else if(y>=32 && y<48)
        printfY = 32;
    else if(y>=48 && y<64)
        printfY = 48;
    else if(y>=64)
        printfY = 48;
    else
        printfY = 0;
#endif
}

void DFRobot_SSD1306::setCursorLine(int8_t line)
{
    line = abs(line);
    if(line == 0) line = 1;
    textLine = line%4;
    if(textLine == 0)
        textLine = 4;
}

void DFRobot_SSD1306::setChCode(const chCode *tp)
{
    tpCode = (chCode *)tp;
}

void DFRobot_SSD1306::print(const char *pCh)
{
    chCode *p = NULL;
    bool findCh;

    while(*pCh != '\0' && pCh != NULL)
    {
        //utf-8 code
        if((*pCh & 0xf0) == 0xe0 && (*(pCh + 1) & 0xc0) == 0x80 && (*(pCh + 2) & 0xc0) == 0x80 || (*pCh == 0xc2 && *(pCh + 1) == 0xb7)) {
            uint32_t utf8;
            if(*pCh == 0xc2 && *(pCh + 1) == 0xb7) {
                utf8 = 0xc2b7;
            }else{
                utf8 = (0x00ff0000 & (uint32_t)*pCh << 16) | (0x0000ff00 & (uint32_t)*(pCh+1) << 8) | (0x000000ff & (uint32_t)*(pCh+2));
            }
            p = tpCode;
            findCh = false;
            if(p){
                while(p->utf8)
                {
                    if(p->utf8 == utf8)
                    {
                        if(printfX > 127){
                            printfY += 16;
                            printfX = 0;
                        }
                        if(printfY > 64)
                            return;
                        showMatrix(printfX,printfY, 16, 16, p->matrix, true);
                        findCh = true;
                        break;
                    }
                    p++;
                }
            }
            if(!findCh){
                chfont* pt = DFFont.listhead;
                while(pt){
                    p = (chCode *)(DFFont.listhead->code);
                    if(p){
                        while(p->utf8){
                            if(p->utf8 == utf8)
                            {
                                if(printfX > 127){
                                    printfY += 16;
                                    printfX = 0;
                                }
                                if(printfY > 64)
                                    return;
                                showMatrix(printfX,printfY, 16, 16, p->matrix, true);
                                findCh = true;
                                break;
                            }
                            p++;
                        }
                    }
                    pt = pt->next;
                }
                //DFFont
            }
            printfX += 16;
            if(*pCh == 0xc2 && *(pCh + 1) == 0xb7) {
                pCh += 2;
            }else{
                pCh += 3;
            }
        //gb2312
        } else if((*pCh & 0x80) == 0x80) {
            uint16_t gb2312 = (0xff00 & (uint16_t)*(pCh) << 8) | (0x00ff & (uint16_t)*(pCh+1));
            p = tpCode;
            findCh = false;
            if(p){
                while(p->gb2312)
                {
                    if(p->gb2312 == gb2312)
                    {
                        if(printfX > 127){
                            printfY += 16;
                            printfX = 0;
                        }
                        if(printfY > 64)
                            return;
                        showMatrix(printfX,printfY, 16, 16, p->matrix, true);
                        findCh = true;
                        break;
                    }
                    p++;
                }
            }
            if(!findCh){
                chfont* pt = DFFont.listhead;
                while(pt){
                    p = (chCode *)(DFFont.listhead->code);
                    if(p){
                        while(p->gb2312){
                            if(p->gb2312 == gb2312)
                            {
                                if(printfX > 127){
                                    printfY += 16;
                                    printfX = 0;
                                }
                                if(printfY > 64)
                                    return;
                                showMatrix(printfX,printfY, 16, 16, p->matrix, true);
                                findCh = true;
                                break;
                            }
                            p++;
                        }
                    }
                    pt = pt->next;
                }
                //DFFont
            }
            printfX += 16;
            pCh += 2;
        //char code
        } else {
            uint8_t ascii = (uint8_t)*pCh -32;
            uint8_t date[16];

            if(printfX > 127){
                printfY += 16;
                printfX = 0;
            }
            if(printfY > 64)
                return;
        
            for(int i=0; i<16; i++)
            {
                #ifdef AVR
                date[i] = pgm_read_byte(&ascii_8_16[ascii][i]);
                #else
               date[i] = ascii_8_16[ascii][i];
                #endif
            }
            if(*pCh == 0x20) showMatrix(printfX,printfY, 8, 16, date, true);
            else showMatrix(printfX,printfY, 8, 16, date, true);
            printfX += 8;
            pCh += 1;
        }
    }
}

void DFRobot_SSD1306::print(double f, int precision)
{
    String str;
    str = String(f,5);

    char *p = (char *)(str.c_str() + str.length());
    while(*p == '\0'|| *p == '0'){
        *p = '\0';
        p--;
    }
    if(*p == '.')
        *p = '\0';
    if(str == "-0")
        str = "0";
    print(str);
}

void DFRobot_SSD1306::print(uint32_t i)
{
    char buffer[34];
    memset(buffer, 0, 34);
    itoa(i, buffer, 10);
    print(buffer);
}

void DFRobot_SSD1306::printLine(double f, int precision)
{
    String str;
    str = String(f,5);
    char *p = (char *)(str.c_str() + str.length());
    while(*p == '\0'|| *p == '0'){
        *p = '\0';
        p--;
    }
    if(*p == '.')
        *p = '\0';
    if(str == "-0")
        str = "0";
    printLine(str);
}

void DFRobot_SSD1306::printLine(uint32_t i)
{
    char buffer[34];
    memset(buffer, 0, 34);
    itoa(i, buffer, 10);
    printLine(buffer);
}

void DFRobot_SSD1306::printLine(const char *pCh)
{
    bool findCh;
    chCode *p = NULL;
    int16_t X=0, Y = 16 * (textLine-1);

#if defined(ESP_PLATFORM) || defined(NRF5)
    for(int i=0; i<128; i++){
        SSD1306_FrameBuffer[(2 * textLine -2) * 128 + i] = 0;
        SSD1306_FrameBuffer[(2 * textLine -1) * 128 + i] = 0;
    }
    for(int i=(2 * textLine -2); i<=(2 * textLine -1); i++)
    {
#if defined(ESP_PLATFORM)
        // send a bunch of data in one xmission
        writeCmd(0xB0 + i);//set page address
        writeCmd((2+0) & 0xf);//set lower column address
        writeCmd(0x10 | ((2+0) >> 4));//set higher column address
#else defined(NRF5)
        writeCmd(SSD1306_COLUMNADDR);
        writeCmd(0);
        writeCmd(127);
        writeCmd(SSD1306_PAGEADDR);
        writeCmd((Y+(i-(2 * textLine -2))*8) / 8);
        writeCmd((Y+(i-(2 * textLine -2))*8 + 8) / 8);
#endif
        for(int j = 0; j < 128/16; j++)
        {
            writeDatBytes(SSD1306_FrameBuffer + i*128 + j*16, 16);
        }
    }
#else
    uint8_t cleanBuffer[128*2];
    memset(cleanBuffer, 0, 128*2);
    writeCmd(SSD1306_COLUMNADDR);
    writeCmd(X);
    writeCmd(X + 128 -1);
    writeCmd(SSD1306_PAGEADDR);
    writeCmd(Y / 8);
    writeCmd(Y/8 +1);
    for(int i=0; i<128*2/16; i++)
        writeDatBytes(cleanBuffer + i*16, 16);
#endif

    while(*pCh != '\0' && pCh != NULL)
    {
        //utf-8 code
        if((*pCh & 0xf0) == 0xe0 && (*(pCh + 1) & 0xc0) == 0x80 && (*(pCh + 2) & 0xc0) == 0x80 || (*pCh == 0xc2 && *(pCh + 1) == 0xb7)) {
            uint32_t utf8;
            if(*pCh == 0xc2 && *(pCh + 1) == 0xb7) {
                utf8 = 0xc2b7;
            }else{
                utf8 = (0x00ff0000 & (uint32_t)*pCh << 16) | (0x0000ff00 & (uint32_t)*(pCh+1) << 8) | (0x000000ff & (uint32_t)*(pCh+2));
            }
            p = tpCode;
            if(p){
                while(p->utf8)
                {
                    if(p->utf8 == utf8)
                    {
                        if(X+16 > 128)
                            return;
                        showMatrix(X,Y, 16, 16, p->matrix, true);
                        findCh = true;
                        break;
                    }
                    p++;
                }
            }
            if(!findCh){
                chfont* pt = DFFont.listhead;
                while(pt){
                    p = (chCode *)(DFFont.listhead->code);
                    if(p){
                        while(p->utf8){
                            if(p->utf8 == utf8)
                            {
                                if(X+16 > 128)
                                    return;
                                showMatrix(X,Y, 16, 16, p->matrix, true);
                                findCh = true;
                                break;
                            }
                            p++;
                        }
                    }
                    pt = pt->next;
                }
                //DFFont
            }
            X += 16;
            if(*pCh == 0xc2 && *(pCh + 1) == 0xb7) {
                pCh += 2;
            }else{
                pCh += 3;
            }
        //gb2312
        } else if((*pCh & 0x80) == 0x80) {
            uint16_t gb2312 = (0xff00 & (uint16_t)*(pCh) << 8) | (0x00ff & (uint16_t)*(pCh+1));
            p = tpCode;
            findCh = false;
            if(p){
                while(p->gb2312)
                {
                    if(p->gb2312 == gb2312)
                    {
                        if(X+16 > 128)
                            return;
                        showMatrix(X,Y, 16, 16, p->matrix, true);

                        break;
                    }
                    p++;
                }
            }
            if(!findCh){
                chfont* pt = DFFont.listhead;
                while(pt){
                    p = (chCode *)(DFFont.listhead->code);
                    if(p){
                        while(p->gb2312){
                            if(p->gb2312 == gb2312)
                            {
                                if(p->gb2312 == gb2312)
                                {
                                    if(X+16 > 128)
                                        return;
                                    showMatrix(X,Y, 16, 16, p->matrix, true);

                                    break;
                                }
                                p++;
                            }
                            p++;
                        }
                    }
                    pt = pt->next;
                }
                //DFFont
            }
            X += 16;
            pCh += 2;
        //char code
        } else {
            uint8_t ascii = (uint8_t)*pCh -32;
            uint8_t date[16];

            if(X+8 > 128)
                return;
        
            for(int i=0; i<16; i++)
            {
                #ifdef AVR
                  date[i] = pgm_read_byte(&ascii_8_16[ascii][i]);
                #else
                date[i] = ascii_8_16[ascii][i];
                #endif
            }
            if(*pCh == 0x20) showMatrix(X,Y, 8, 16, date, true);
            else showMatrix(X,Y, 8, 16, date, true);
            X += 8;
            pCh += 1;
        }
    }
}

#if defined(ESP_PLATFORM) || defined(NRF5)

void DFRobot_SSD1306::setCursorXY(int16_t x, int16_t y)
{
    if(x > 127) {
        printfX = 127;
    } else {
        printfX = x;
    }
    if(y > 63) {
        printfY = 63;
    } else {
        printfY = y;
    }
}

int DFRobot_SSD1306::ternaryExpression(bool condition, int o1, int o2)
{
    if(condition)
        return o1;
    return o2;
}

int DFRobot_SSD1306::getDirection(int value)
{
    if (value >= 0)
        return 1;
    return -1;
}

void DFRobot_SSD1306::point(int16_t x, int16_t y, uint8_t color)
{
    x = max(min(x, 127), 0);
    y = max(min(y, 63), 0);
    writeBufferPixel(x, y, color);
    uint8_t    pageaddr = y/8;
#if defined(ESP_PLATFORM)
    // send a bunch of data in one xmission
    writeCmd(0xB0 + pageaddr);//set page address
    writeCmd((2+x) & 0xf);//set lower column address
    writeCmd(0x10 | ((2+x) >> 4));//set higher column address
#else defined(NRF5)
    writeCmd(SSD1306_COLUMNADDR);
    writeCmd(x);
    writeCmd(x);
    writeCmd(SSD1306_PAGEADDR);
    writeCmd(pageaddr);
    writeCmd(pageaddr);
#endif
    writeDatBytes(SSD1306_FrameBuffer + pageaddr*128 + x, 1);
}

void DFRobot_SSD1306::VLine(int16_t x, int16_t y, int16_t h)
{
    x = int(x);
    y = int(y);
    h = int(h);
    int direction = getDirection(h);
    x -= _lineWidth / 2;
    h = ternaryExpression(h > 0, h, -h);
    int xx;
    for (int i=0; i<ternaryExpression(h > 0, h, - h); i++)
    {
        xx = x;
        for(int j=0; j<_lineWidth; j++)
        {
            writeBufferPixel(xx, y, 1);
            xx += 1;
        }
        y += direction;
    }
}

void DFRobot_SSD1306::HLine(int16_t x, int16_t y, int16_t w)
{
    x = int(x);
    y = int(y);
    w = int(w);
    int direction = getDirection(w);
    y -= _lineWidth / 2;
    int yy;
    for(int i=0; i<ternaryExpression(w > 0, w, - w); i++)
    {
        yy = y;
        for(int j=0; j<_lineWidth; j++)
        {
            writeBufferPixel(x, yy, 1);
            yy += 1;
        }
        x += direction;
    }
}

void DFRobot_SSD1306::line(int16_t x0, int16_t y0, int16_t x1, int16_t y1)
{
    x0 = int(x0);
    y0 = int(y0);
    x1 = int(x1);
    y1 = int(y1);
    if(x0 == x1){
      VLine(x0, y0, y1 - y0 + 1);
    }else if(y0 == y1){
      HLine(x0, y0, x1 - x0 + 1);
    }else{
        uint8_t dx = abs(x1 - x0);
        uint8_t dy = abs(y1 - y0);
        int dirX = ternaryExpression(x0 < x1, 1, -1);
        int dirY = ternaryExpression(y0 < y1, 1, -1);
        int err;
        if(dx > dy){
            err = dx / 2;
            for(int i=0; i<dx; i++)
            {
                HLine(x0, y0, 1);
                x0 += dirX;
                err -= dy;
                if(err < 0)
                {
                    err += dx;
                    y0 += dirY;
                }
            }
            HLine(x1, y1, 1);
        }
        else
        {
            err = dy / 2;
            for(int i=0; i<dy; i++){
                VLine(x0, y0, 1);
                y0 += dirY;
                err -= dx;
                if(err < 0)
                {
                    err += dy;
                    x0 += dirX;
                }
            }
            VLine(x1, y1, 1);
        }
    }

    byte i, j;
    for ( i = 0; i < 8; i++)
    {
#if defined(ESP_PLATFORM)
        // send a bunch of data in one xmission
        writeCmd(0xB0 + i);//set page address
        writeCmd((2+0) & 0xf);//set lower column address
        writeCmd(0x10 | ((2+0) >> 4));//set higher column address
#else defined(NRF5)
        writeCmd(SSD1306_COLUMNADDR);
        writeCmd(0);
        writeCmd(127);
        writeCmd(SSD1306_PAGEADDR);
        writeCmd(i);
        writeCmd(i+1);
#endif
        for( j = 0; j < 8; j++)
        {
            writeDatBytes(SSD1306_FrameBuffer + i*128 + j*16, 16);
        }
    }
}

void DFRobot_SSD1306::rect(int16_t x, int16_t y, int16_t width, int16_t height, bool fill)
{
    if(width == 0 && height == 0){
        return;
    }else if(width == 0){
        VLine(x,y,height);
    }else if(height == 0){
        HLine(x,y,width);
    }else if(fill){
        int temp = _lineWidth;
        _lineWidth = 1;
        if (width < 0)
        {
            x += width;
            width = abs(width);
        }
        for(int i=0; i<width; i++)
            VLine(x + i, y, height);
        _lineWidth = temp;
    }else{
        if(width < 0)
        {
            x += width;
            width = -width;
        }
        if(height < 0)
        {
            y += height;
            height = -height;
        }
        HLine(x - _lineWidth / 2, y, width + _lineWidth);
        HLine(x - _lineWidth / 2, y + height, width + _lineWidth);
        VLine(x, y - _lineWidth / 2, height + _lineWidth);
        VLine(x + width, y - _lineWidth / 2, height + _lineWidth);
    }

    byte i, j;
    for ( i = 0; i < 8; i++)
    {
#if defined(ESP_PLATFORM)
        // send a bunch of data in one xmission
        writeCmd(0xB0 + i);//set page address
        writeCmd((2+0) & 0xf);//set lower column address
        writeCmd(0x10 | ((2+0) >> 4));//set higher column address
#else defined(NRF5)
        writeCmd(SSD1306_COLUMNADDR);
        writeCmd(0);
        writeCmd(127);
        writeCmd(SSD1306_PAGEADDR);
        writeCmd(i);
        writeCmd(i+1);
#endif
        for( j = 0; j < 8; j++)
        {
            writeDatBytes(SSD1306_FrameBuffer + i*128 + j*16, 16);
        }
    }
}

void DFRobot_SSD1306::drawImage(int16_t x, int16_t y, int16_t width, int16_t height, const uint8_t *image)
{
    if(width==0 && height==0 || !image) return;
    uint8_t _x = x, _y = y;

    for(uint8_t j=0; j<height; j++){
        if(_y > 63) break;
        for(uint8_t i=0; i<width/8; i++)
        {
            uint8_t data = *image;//Serial.println(data,HEX);
            for(uint8_t k=0; k<8; k++)
            {
                if(_x>127) break;
                if(data & 0x80)
                    writeBufferPixel(_x,_y,1);
                else
                    writeBufferPixel(_x,_y,0);
                data = data << 1;
                _x++;
            }
            image++;
            if(!image) return;
        }
        if(width%8){
            uint8_t data = *image;
            for(uint8_t i=0; i<width%8; i++)
            {
                if(_x>127) break;
                if(data & 0x80)
                    writeBufferPixel(_x,_y,1);
                else
                    writeBufferPixel(_x,_y,0);
                data = data << 1;
                _x++;
            }
            image++;
            if(!image) return;
        }
        _y++;_x = x;
    }

    byte i, j;

    for ( i = 0; i < 8; i++)
    {
#if defined(ESP_PLATFORM)
        // send a bunch of data in one xmission
        writeCmd(0xB0 + i);//set page address
        writeCmd((2+0) & 0xf);//set lower column address
        writeCmd(0x10 | ((2+0) >> 4));//set higher column address
#else defined(NRF5)
        writeCmd(SSD1306_COLUMNADDR);
        writeCmd(0);
        writeCmd(127);
        writeCmd(SSD1306_PAGEADDR);
        writeCmd((y+i*8) / 8);
        writeCmd((y+i*8 + 8) / 8);
#endif
        for( j = 0; j < 8; j++)
        {
            writeDatBytes(SSD1306_FrameBuffer + i*128 + j*16, 16);
        }
    }
}
void DFRobot_SSD1306::lineWidth(uint8_t w)
{
    _lineWidth = w;
}

void DFRobot_SSD1306::writeBufferPixel(int16_t x, int16_t y, uint16_t color)
{
    if(x<0||x>127||y<0||y>63) return;
    x = max(min(x, 127), 0);
    y = max(min(y, 63), 0);
    int16_t addr = x + y / 8 * 128;
    if(color) {
        SSD1306_FrameBuffer[addr] |= 0x01 << (y % 8);
    } else {
        SSD1306_FrameBuffer[addr] &= ~ (0x01 << (y % 8));
    }
}

void DFRobot_SSD1306::regionalRefresh(int16_t x, int16_t y, int16_t w, int16_t h)
{
    int16_t xe = x+w-1;
    int16_t ye = y+h-1;

    if(x<0) x=0;
    if(x>127) x=127;
    if(y<0) y=0;
    if(y>63) y=63;

    if(xe<0) xe=0;
    if(xe>127) xe=127;
    if(ye<0) ye=0;
    if(ye>63) ye=63;

    int16_t data;
    if(xe < x) {
        data = xe;
        xe = x;
        x = data;
    }
    if(ye < y) {
        data = ye;
        ye = y;
        y = data;
    }
    w = xe - x + 1;
    h = ye - y + 1;

    int8_t widthSize = (xe-x+1)/16;
    int8_t widthSizeRemainder = (xe-x+1)%16;
    int8_t pageaddrLow = y/8;
    int8_t pageaddrHight = ye/8;

    for(uint8_t i=pageaddrLow; i<=pageaddrHight; i++)
    {
#if defined(ESP_PLATFORM)
        // send a bunch of data in one xmission
        writeCmd(0xB0 + i);//set page address
        writeCmd((2+x) & 0xf);//set lower column address
        writeCmd(0x10 | ((2+x) >> 4));//set higher column address
#else defined(NRF5)
        writeCmd(SSD1306_COLUMNADDR);
        writeCmd(x);
        writeCmd(x + w - 1);
        writeCmd(SSD1306_PAGEADDR);
        writeCmd((y+(i-pageaddrLow)*8) / 8);
        writeCmd((y+(i-pageaddrLow)*8 + 8) / 8);
#endif
        uint8_t j;
        for(j = 0; j < widthSize; j++)
        {
            writeDatBytes(SSD1306_FrameBuffer + i*128 + x + j*16, 16);
        }
        if(widthSizeRemainder)
        {
            writeDatBytes(SSD1306_FrameBuffer + i*128 + x + j*16, widthSizeRemainder);
        }
    }
}

void DFRobot_SSD1306::drawCircle(int16_t x0, int16_t y0, int16_t r)
{
  r = abs(r);
  int16_t       varX = 0, varY = r;
  int16_t       endY = 0;
  int32_t       var1 = 3 - 2 * r;

  while(varX <= varY) {
    writeBufferPixel(x0 + varX, y0 + varY, 1);
    writeBufferPixel(x0 - varX, y0 + varY, 1);
    writeBufferPixel(x0 + varX, y0 - varY, 1);
    writeBufferPixel(x0 - varX, y0 - varY, 1);
    writeBufferPixel(x0 + varY, y0 + varX, 1);
    writeBufferPixel(x0 - varY, y0 + varX, 1);
    writeBufferPixel(x0 + varY, y0 - varX, 1);
    writeBufferPixel(x0 - varY, y0 - varX, 1);
    if(var1 < 0) {
      var1 = var1 + 4 * varX + 6;
    } else {
      var1 = var1 + 4 * (varX - varY) + 10;
      varY --;
    }
    varX ++;
  }
  regionalRefresh(x0-r, y0-r, 2*r+1, 2*r+1);
}

void DFRobot_SSD1306::fillCircle(int16_t x0, int16_t y0, int16_t r)
{ 
  r = abs(r);
  int16_t       varX = 0, varY = r;
  int16_t       endY = 0;
  int32_t       var1 = 3 - 2 * r;

  while(varX <= varY) {
    drawVLine(x0+varX, y0-varY, 2 * varY + 1);
    drawVLine(x0+varY, y0-varX, 2 * varX + 1);
    drawVLine(x0-varX, y0-varY, 2 * varY + 1);
    drawVLine(x0-varY, y0-varX, 2 * varX + 1);
    if(var1 < 0) {
      var1 = var1 + 4 * varX + 6;
    } else {
      var1 = var1 + 4 * (varX - varY) + 10;
      varY --;
    }
    varX ++;
  }
  regionalRefresh(x0-r, y0-r, 2*r+1, 2*r+1);
}

void DFRobot_SSD1306::drawVLine(int16_t x, int16_t y, int16_t height_)
{
  int8_t        direction = 1;
  int16_t       var1 = y + height_;
  if(height_ < 0) {
    direction = -1;
  }
  for(; y != var1; y += direction) {
    writeBufferPixel(x, y, 1);
  }
}

#endif


