#define OLED_HEIGHT_64          0x12
#define OLED_64                 0x3F

#define OLED_DISPLAY_OFF        0xAE
#define OLED_DISPLAY_ON         0xAF

#define OLED_COMMAND_MODE       0x00
#define OLED_ONE_COMMAND_MODE   0x80
#define OLED_DATA_MODE          0x40

#define OLED_ADDRESSING_MODE    0x20
#define OLED_VERTICAL           0x01

#define OLED_NORMAL_V            0xC8
#define OLED_NORMAL_H            0xA1

#define OLED_CONTRAST           0x81
#define OLED_SETCOMPINS           0xDA
#define OLED_SETVCOMDETECT        0xDB
#define OLED_CLOCKDIV             0xD5
#define OLED_SETMULTIPLEX        0xA8
#define OLED_COLUMNADDR            0x21
#define OLED_PAGEADDR            0x22
#define OLED_CHARGEPUMP            0x8D

#define OLED_NORMALDISPLAY        0xA6

#define OLED_BUFSIZE (128*64/8)

#define OLED_MAX_X 127
#define OLED_MAX_Y 63
#define OLED_MAX_ROW 7

#define OLED_ADDR 0x3C
#define OLED_I2C_FREQ 100000UL


uint8_t _oled_buffer[OLED_BUFSIZE];
uint8_t _writes = 0;


void i2c_begin(uint8_t address) {
    TWBR = ((F_CPU / OLED_I2C_FREQ) - 16) / 2;
    TWAR = (address << 1);
}

void i2c_endTransaction() {
    TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
}

void i2c_beginTransmission(uint8_t address) {
    TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
    while (!(TWCR & (1 << TWINT)));

    TWDR = (address << 1);
    TWCR = (1 << TWINT) | (1 << TWEN);
    while (!(TWCR & (1 << TWINT)));
}

void i2c_write(uint8_t data) {
    TWDR = data;  // Запись данных
    TWCR = (1 << TWINT) | (1 << TWEN);
    while (!(TWCR & (1 << TWINT)));
}





void sendByte(uint8_t data) {
    sendByteRaw(data);
    _writes++;
    if (_writes >= 16) {
        endTransm();
        beginData();
    }
}

void sendByteRaw(uint8_t data) {
    i2c_write(data);
}

void sendCommand(uint8_t cmd1) {
    beginOneCommand();
    sendByteRaw(cmd1);
    endTransm();
}

void sendCommand(uint8_t cmd1, uint8_t cmd2) {
    beginCommand();
    sendByteRaw(cmd1);
    sendByteRaw(cmd2);
    endTransm();
}

void setWindow(int x0, int y0, int x1, int y1) {
    beginCommand();
    sendByteRaw(OLED_COLUMNADDR);
    sendByteRaw(constrain(x0, 0, OLED_MAX_X));
    sendByteRaw(constrain(x1, 0, OLED_MAX_X));
    sendByteRaw(OLED_PAGEADDR);
    sendByteRaw(constrain(y0, 0, OLED_MAX_ROW));
    sendByteRaw(constrain(y1, 0, OLED_MAX_ROW));
    endTransm();
}

void beginData() {
    startTransm();
    sendByteRaw(OLED_DATA_MODE);
}

void beginCommand() {
    startTransm();
    sendByteRaw(OLED_COMMAND_MODE);
}

void beginOneCommand() {
    startTransm();
    sendByteRaw(OLED_ONE_COMMAND_MODE);
}

void endTransm() {
    i2c_endTransaction();
    _writes = 0;
}

void startTransm() {
    i2c_beginTransmission(OLED_ADDR);
}

void initialization() {
    i2c_begin(OLED_ADDR);

    beginCommand();
    sendByte(OLED_DISPLAY_OFF);
    sendByte(OLED_CLOCKDIV);
    sendByte(0x80);
    sendByte(OLED_CHARGEPUMP);
    sendByte(0x14);
    sendByte(OLED_ADDRESSING_MODE);
    sendByte(OLED_VERTICAL);
    sendByte(OLED_NORMAL_H);
    sendByte(OLED_NORMAL_V);
    sendByte(OLED_CONTRAST);
    sendByte(0x7F);
    sendByte(OLED_SETVCOMDETECT);
    sendByte(0x40);
    sendByte(OLED_NORMALDISPLAY);
    sendByte(OLED_DISPLAY_ON);
    endTransm();

    beginCommand();
    sendByte(OLED_SETCOMPINS);
    sendByte(OLED_HEIGHT_64);
    sendByte(OLED_SETMULTIPLEX);
    sendByte(OLED_64);
    endTransm();

    setWindow(0, 0, OLED_MAX_X, OLED_MAX_ROW);
}

void setBit(uint8_t &value, uint8_t bitIndex, bool bitValue) {
    if (bitValue) {
        value |= (1 << bitIndex);
    } else {
        value &= ~(1 << bitIndex);
    }
}

void DrawPixel(uint8_t x, uint8_t y, uint8_t fill = 1) {
    if (x < 0 || x > OLED_MAX_X || y < 0 || y > OLED_MAX_Y) return;
    uint16_t _bufIndex = ((y) >> 3) + ((x) << (3));
    setBit(_oled_buffer[_bufIndex], y & 0b111, fill);
}

void DrawLine(uint8_t x1, uint8_t y1, uint8_t x2, uint8_t y2, uint8_t color = 1) {
    uint8_t dx = abs(x2 - x1);
    uint8_t dy = abs(y2 - y1);
    uint8_t sx = x1 < x2 ? 1 : -1;
    uint8_t sy = y1 < y2 ? 1 : -1;
    uint8_t err = dx - dy;

    while (true) {
        DrawPixel(x1, y1, color);

        if (x1 == x2 && y1 == y2) break;

        uint8_t e2 = 2 * err;

        if (e2 > -dy) {
            err -= dy;
            x1 += sx;
        }

        if (e2 < dx) {
            err += dx;
            y1 += sy;
        }
    }
}

void DrawRect(uint8_t x, uint8_t y, uint8_t width, uint8_t height, uint8_t color = 1, uint8_t fill = 0) {
    if (fill) {
        for (uint8_t i = y; i < y + height; i++) {
            DrawLine(x, i, x + width - 1, i, color);
        }
    } else {
        for (uint8_t i = x; i < x + width; i++) {
            DrawPixel(i, y, color);
            DrawPixel(i, y + height - 1, color);
        }
        for (uint8_t i = y; i < y + height; i++) {
            DrawPixel(x, i, color);
            DrawPixel(x + width - 1, i, color);
        }
    }
}

//
void DrawCircle(uint8_t x0, uint8_t y0, uint8_t radius, uint8_t color = 1, uint8_t fill = 0) {
    int8_t x = radius;
    int8_t y = 0;
    int8_t err = 0;

    while (x >= y) {
        if (fill) {
            for (int8_t i = x0 - x; i <= x0 + x; i++) {
                DrawPixel(i, y0 + y, color);
                DrawPixel(i, y0 - y, color);
            }
            for (int8_t i = x0 - y; i <= x0 + y; i++) {
                DrawPixel(i, y0 + x, color);
                DrawPixel(i, y0 - x, color);
            }
        } else {
            DrawPixel(x0 + x, y0 + y, color);
            DrawPixel(x0 - x, y0 + y, color);
            DrawPixel(x0 + x, y0 - y, color);
            DrawPixel(x0 - x, y0 - y, color);
            DrawPixel(x0 + y, y0 + x, color);
            DrawPixel(x0 - y, y0 + x, color);
            DrawPixel(x0 + y, y0 - x, color);
            DrawPixel(x0 - y, y0 - x, color);
        }

        y++;
        err += 1 + 2 * y;
        if (2 * (err - x) + 1 > 0) {
            x--;
            err += 1 - 2 * x;
        }
    }
}

// Заполнить дисплей
void Fill(uint8_t fill = 0) {
    memset(_oled_buffer, fill, OLED_BUFSIZE);
}

// Обновить дисплей
void update() {
    setWindow(0, 0, OLED_MAX_X, OLED_MAX_ROW);
    beginData();
    for (int i = 0; i < OLED_BUFSIZE; i++) sendByte(_oled_buffer[i]);
    endTransm();
}

void setup() {
    initialization();
    Fill();
    DrawCircle(50, 30, 20, 1, 1);
}

void loop() {
    delay(30);
    update();
}