#include "i2c_oled.h"

//////////////////////////////////////////////////////////
/* Absolute value */
#define ABS(x)   ((x) > 0 ? (x) : -(x))

/* SSD1306 data buffer */
static uint8_t SSD1306_Buffer[SSD1306_WIDTH * SSD1306_HEIGHT / 8];

uint16_t I2C_TXdata[16];
uint16_t I2C_RXdata[16];
/* Private SSD1306 structure */
typedef struct {
    uint16_t CurrentX;
    uint16_t CurrentY;
    uint8_t Inverted;
    uint8_t Initialized;
} SSD1306_t;

/* Private variable */
static SSD1306_t SSD1306;
/* Функция  инициализации дисплея подает управялющие комманды на микросхему управления sh1106 */
uint8_t SSD1306_Init(void) {

    /* A little delay */
    uint32_t p = 2500;
    while(p>0)
        p--;

    /* Init LCD */
    SSD1306_WRITECOMMAND(0xAE); //display off

    SSD1306_WRITECOMMAND(0xB0); //Set Page  Address
    SSD1306_WRITECOMMAND(0x40); //---set high column address
    SSD1306_WRITECOMMAND(0x00); //---set low column address

    SSD1306_WRITECOMMAND(0xA4); //---set high column address

    SSD1306_WRITECOMMAND(0xD5); //--set start line address
    SSD1306_WRITECOMMAND(0x50); //--set contrast control register

    SSD1306_WRITECOMMAND(0xA8);
    SSD1306_WRITECOMMAND(0x3F); //--set segment re-map 0 to 127

    SSD1306_WRITECOMMAND(0xD3); //--set normal display
    SSD1306_WRITECOMMAND(0x00); //--set multiplex ratio(1 to 64)

    SSD1306_WRITECOMMAND(0x40); //

    SSD1306_WRITECOMMAND(0xAD); //0xa4,Output follows RAM content;0xa5,Output ignores RAM content
    SSD1306_WRITECOMMAND(0x8A); //-set display offset

    SSD1306_WRITECOMMAND(0xA1); //-not offset

    SSD1306_WRITECOMMAND(0xC8); //--set display clock divide ratio/oscillator frequency

    SSD1306_WRITECOMMAND(0xDA); //--set divide ratio
    SSD1306_WRITECOMMAND(0x12); //--set pre-charge period

    SSD1306_WRITECOMMAND(0x81); //
    SSD1306_WRITECOMMAND(0xFF); //--set com pins hardware configuration

    SSD1306_WRITECOMMAND(0xD9);
    SSD1306_WRITECOMMAND(0x11); //--set vcomh

    SSD1306_WRITECOMMAND(0xDB); //0x20,0.77xVcc
    SSD1306_WRITECOMMAND(0x35); //--set DC-DC enable

    SSD1306_WRITECOMMAND(0xA6); //

    SSD1306_WRITECOMMAND(0xAF); //--turn on SSD1306 panel

    /* Clear screen */
    SSD1306_Fill(SSD1306_COLOR_BLACK);

    /* Update screen */
    SSD1306_UpdateScreen();
    /* Set default values */
    SSD1306.CurrentX = 0;
    SSD1306.CurrentY = 0;

    /* Initialized OK */
    SSD1306.Initialized = 1;

    /* Return OK */
    return 1;
}

//Начальные адресса колонки и странцы 
#define SSD1306_COLUMNADDR              0x21
#define SSD1306_PAGEADDR                0xB0

//Функция перемещения на начльную позцию 
void SSD1306_setPosition(uint8_t column, uint8_t page) {
    if (column > SSD1306_WIDTH - 1) {
        column = 0;                                    // Ограничение столбца
    }
    if (page > 7) {
        page = 0;                                      // Ограничение страницы
    }

    SSD1306_WRITECOMMAND(0x20+column);                   // Начальный адрес столбца
    SSD1306_WRITECOMMAND(SSD1306_PAGEADDR+page);                     // Начальный адрес страницы
  //  SSD1306_WRITECOMMAND(7);                        // Конечный адрес страницы
}

//Функция отображения картинки из буффера
void SSD1306_UpdateScreen(void) {

       uint8_t page;
       uint8_t column;
       uint8_t data_bytes = SSD1306_WIDTH / 8;
       int i;
       for (page = 0; page < 8; page++) {
           SSD1306_WRITECOMMAND(0x10);
           SSD1306_WRITECOMMAND(0x00);
           for (column = 0; column < SSD1306_WIDTH; column++) { //
           SSD1306_setPosition(column, page);  // Начало столбца - 0, страница - m

           // Запись данных в буфер дисплея
           I2C_TXdata[0] = 0x40;  // Байт управления: данные
           // Заполнить буфер I2C данными из буфера дисплея
                      I2C_TXdata[0] = 0x40;  // Команда записи данных
                      I2C_TXdata[1] = SSD1306_Buffer[SSD1306_WIDTH*page + column];
           I2CWrite(I2C_SLAVE_ADDRESS, 2, true);
       }
   }
}
//Функция инвертирования бфуффера.
void SSD1306_ToggleInvert(void) {
    uint16_t i;

    /* Toggle invert */
    SSD1306.Inverted = !SSD1306.Inverted;

    /* Do memory toggle */
    for (i = 0; i < sizeof(SSD1306_Buffer); i++) {
        SSD1306_Buffer[i] = ~SSD1306_Buffer[i];
    }
}
// Функция заливки (стирания)
void SSD1306_Fill(SSD1306_COLOR_t color) {
    /* Set memory */
    memset(SSD1306_Buffer, (color == SSD1306_COLOR_BLACK) ? 0x00 : 0xFF, sizeof(SSD1306_Buffer));
}
void SSD1306_DrawPixel(uint16_t x, uint16_t y, SSD1306_COLOR_t color) {
    if (x >= SSD1306_WIDTH || y >= SSD1306_HEIGHT) {
        return;
    }

    if (SSD1306.Inverted) {
        color = (SSD1306_COLOR_t)!color;
    }

    // Правильно вычисляем индекс байта в буфере
    uint16_t byteIndex = x + (y / 8) * SSD1306_WIDTH;
    // Правильно вычисляем индекс бита в байт
    uint8_t bitIndex = y % 8;

    if (color == SSD1306_COLOR_WHITE) {
        SSD1306_Buffer[byteIndex] |= (1 << bitIndex);
    } else {
        SSD1306_Buffer[byteIndex] &= ~(1 << bitIndex);
    }
}

/* Функция отображения растрового изображения */
void ssd1306_DrawBitmap(uint8_t x, uint8_t y, const unsigned char* bitmap, uint8_t w, uint8_t h, SSD1306_COLOR_t color) {
    int16_t byteWidth = (w + 7) / 8; // Bitmap scanline pad = whole byte
    uint8_t byte = 0;
    uint8_t i,j;
    if (x >= SSD1306_WIDTH || y >= SSD1306_HEIGHT) {
        return;
    }

    for ( j = 0; j < h; j++, y++) {
        for ( i = 0; i < w; i++) {
            if (i & 7) {
                byte <<= 1;
            } else {
                byte = (*(const unsigned char *)(&bitmap[j * byteWidth + i / 8]));
            }

            if (byte & 0x80) {
                SSD1306_DrawPixel(x + i, y, color);
            }
        }
    }
    return;
}

// функция позцианирования каретки по изображению
void SSD1306_GotoXY(uint16_t x, uint16_t y) {
    /* Set write pointers */
    SSD1306.CurrentX = x;
    SSD1306.CurrentY = y;
}
// функция отображения символов 
char SSD1306_Putc(char ch, FontDef_t* Font, SSD1306_COLOR_t color) {
    uint32_t i, b, j;

    /* Check available space in LCD */
    if (
        SSD1306_WIDTH <= (SSD1306.CurrentX + Font->FontWidth) ||
        SSD1306_HEIGHT <= (SSD1306.CurrentY + Font->FontHeight)
    ) {
        /* Error */
        return 0;
    }

    /* Go through 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_t) color);
            } else {
                SSD1306_DrawPixel(SSD1306.CurrentX + j, (SSD1306.CurrentY + i), (SSD1306_COLOR_t)!color);
            }
        }
    }

    /* Increase pointer */
    SSD1306.CurrentX += Font->FontWidth;

    /* Return character written */
    return ch;
}
//Диспетчер отображения строк.
char SSD1306_Puts(char* str, FontDef_t* Font, SSD1306_COLOR_t color) {
    /* Write characters */
   // reverseString(str);
    while (*str) {
        /* Write character by character */
        if (SSD1306_Putc(*str, Font, color) != *str) {
            /* Return error */
            return *str;
        }

        /* Increase string pointer */
        str++;
    }

    /* Everything OK, zero should be returned */
    return *str;
}

//Функция построения прямых линий по алгоритму брезинхема
void SSD1306_DrawLine(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, SSD1306_COLOR_t c) {
    int16_t dx, dy, sx, sy, err, e2, i, tmp;

    /* Check for overflow */
    if (x0 >= SSD1306_WIDTH) {
        x0 = SSD1306_WIDTH - 1;
    }
    if (x1 >= SSD1306_WIDTH) {
        x1 = SSD1306_WIDTH - 1;
    }
    if (y0 >= SSD1306_HEIGHT) {
        y0 = SSD1306_HEIGHT - 1;
    }
    if (y1 >= SSD1306_HEIGHT) {
        y1 = SSD1306_HEIGHT - 1;
    }

    dx = (x0 < x1) ? (x1 - x0) : (x0 - x1);
    dy = (y0 < y1) ? (y1 - y0) : (y0 - y1);
    sx = (x0 < x1) ? 1 : -1;
    sy = (y0 < y1) ? 1 : -1;
    err = ((dx > dy) ? dx : -dy) / 2;

    if (dx == 0) {
        if (y1 < y0) {
            tmp = y1;
            y1 = y0;
            y0 = tmp;
        }

        if (x1 < x0) {
            tmp = x1;
            x1 = x0;
            x0 = tmp;
        }

        /* Vertical line */
        for (i = y0; i <= y1; i++) {
            SSD1306_DrawPixel(x0, i, c);
        }

        /* Return from function */
        return;
    }

    if (dy == 0) {
        if (y1 < y0) {
            tmp = y1;
            y1 = y0;
            y0 = tmp;
        }

        if (x1 < x0) {
            tmp = x1;
            x1 = x0;
            x0 = tmp;
        }

        /* Horizontal line */
        for (i = x0; i <= x1; i++) {
            SSD1306_DrawPixel(i, y0, c);
        }

        /* Return from function */
        return;
    }

    while (1) {
        SSD1306_DrawPixel(x0, y0, c);
        if (x0 == x1 && y0 == y1) {
            break;
        }
        e2 = err;
        if (e2 > -dx) {
            err -= dy;
            x0 += sx;
        }
        if (e2 < dy) {
            err += dx;
            y0 += sy;
        }
    }
}
//функция построения прямоугольников
void SSD1306_DrawRectangle(uint16_t x, uint16_t y, uint16_t w, uint16_t h, SSD1306_COLOR_t c) {
    /* Check input parameters */
    if (
        x >= SSD1306_WIDTH ||
        y >= SSD1306_HEIGHT
    ) {
        /* Return error */
        return;
    }

    /* Check width and height */
    if ((x + w) >= SSD1306_WIDTH) {
        w = SSD1306_WIDTH - x;
    }
    if ((y + h) >= SSD1306_HEIGHT) {
        h = SSD1306_HEIGHT - y;
    }

    /* Draw 4 lines */
    SSD1306_DrawLine(x, y, x + w, y, c);         /* Top line */
    SSD1306_DrawLine(x, y + h, x + w, y + h, c); /* Bottom line */
    SSD1306_DrawLine(x, y, x, y + h, c);         /* Left line */
    SSD1306_DrawLine(x + w, y, x + w, y + h, c); /* Right line */
}
//Функция построения прямоугольников закрашенных прямоугольников
void SSD1306_DrawFilledRectangle(uint16_t x, uint16_t y, uint16_t w, uint16_t h, SSD1306_COLOR_t c) {
    uint8_t i;

    /* Check input parameters */
    if (
        x >= SSD1306_WIDTH ||
        y >= SSD1306_HEIGHT
    ) {
        /* Return error */
        return;
    }

    /* Check width and height */
    if ((x + w) >= SSD1306_WIDTH) {
        w = SSD1306_WIDTH - x;
    }
    if ((y + h) >= SSD1306_HEIGHT) {
        h = SSD1306_HEIGHT - y;
    }

    /* Draw lines */
    for (i = 0; i <= h; i++) {
        /* Draw lines */
        SSD1306_DrawLine(x, y + i, x + w, y + i, c);
    }
}
//Фунция построения треугольников
void SSD1306_DrawTriangle(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, uint16_t x3, uint16_t y3, SSD1306_COLOR_t color) {
    /* Draw lines */
    SSD1306_DrawLine(x1, y1, x2, y2, color);
    SSD1306_DrawLine(x2, y2, x3, y3, color);
    SSD1306_DrawLine(x3, y3, x1, y1, color);
}

//Фунция построения закрашенных треугольников
void SSD1306_DrawFilledTriangle(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, uint16_t x3, uint16_t y3, SSD1306_COLOR_t color) {
    int16_t deltax = 0, deltay = 0, x = 0, y = 0, xinc1 = 0, xinc2 = 0,
    yinc1 = 0, yinc2 = 0, den = 0, num = 0, numadd = 0, numpixels = 0,
    curpixel = 0;

    deltax = ABS(x2 - x1);
    deltay = ABS(y2 - y1);
    x = x1;
    y = y1;

    if (x2 >= x1) {
        xinc1 = 1;
        xinc2 = 1;
    } else {
        xinc1 = -1;
        xinc2 = -1;
    }

    if (y2 >= y1) {
        yinc1 = 1;
        yinc2 = 1;
    } else {
        yinc1 = -1;
        yinc2 = -1;
    }

    if (deltax >= deltay){
        xinc1 = 0;
        yinc2 = 0;
        den = deltax;
        num = deltax / 2;
        numadd = deltay;
        numpixels = deltax;
    } else {
        xinc2 = 0;
        yinc1 = 0;
        den = deltay;
        num = deltay / 2;
        numadd = deltax;
        numpixels = deltay;
    }

    for (curpixel = 0; curpixel <= numpixels; curpixel++) {
        SSD1306_DrawLine(x, y, x3, y3, color);

        num += numadd;
        if (num >= den) {
            num -= den;
            x += xinc1;
            y += yinc1;
        }
        x += xinc2;
        y += yinc2;
    }
}
//Функция построения окружностей
void SSD1306_DrawCircle(int16_t x0, int16_t y0, int16_t r, SSD1306_COLOR_t c) {
    int16_t f = 1 - r;
    int16_t ddF_x = 1;
    int16_t ddF_y = -2 * r;
    int16_t x = 0;
    int16_t y = r;

    SSD1306_DrawPixel(x0, y0 + r, c);
    SSD1306_DrawPixel(x0, y0 - r, c);
    SSD1306_DrawPixel(x0 + r, y0, c);
    SSD1306_DrawPixel(x0 - r, y0, c);

    while (x < y) {
        if (f >= 0) {
            y--;
            ddF_y += 2;
            f += ddF_y;
        }
        x++;
        ddF_x += 2;
        f += ddF_x;

        SSD1306_DrawPixel(x0 + x, y0 + y, c);
        SSD1306_DrawPixel(x0 - x, y0 + y, c);
        SSD1306_DrawPixel(x0 + x, y0 - y, c);
        SSD1306_DrawPixel(x0 - x, y0 - y, c);

        SSD1306_DrawPixel(x0 + y, y0 + x, c);
        SSD1306_DrawPixel(x0 - y, y0 + x, c);
        SSD1306_DrawPixel(x0 + y, y0 - x, c);
        SSD1306_DrawPixel(x0 - y, y0 - x, c);
    }
}
//Функция построения закрашенных окружностей
void SSD1306_DrawFilledCircle(int16_t x0, int16_t y0, int16_t r, SSD1306_COLOR_t c) {
    int16_t f = 1 - r;
    int16_t ddF_x = 1;
    int16_t ddF_y = -2 * r;
    int16_t x = 0;
    int16_t y = r;

    SSD1306_DrawPixel(x0, y0 + r, c);
    SSD1306_DrawPixel(x0, y0 - r, c);
    SSD1306_DrawPixel(x0 + r, y0, c);
    SSD1306_DrawPixel(x0 - r, y0, c);
    SSD1306_DrawLine(x0 - r, y0, x0 + r, y0, c);

    while (x < y) {
        if (f >= 0) {
            y--;
            ddF_y += 2;
            f += ddF_y;
        }
        x++;
        ddF_x += 2;
        f += ddF_x;

        SSD1306_DrawLine(x0 - x, y0 + y, x0 + x, y0 + y, c);
        SSD1306_DrawLine(x0 + x, y0 - y, x0 - x, y0 - y, c);

        SSD1306_DrawLine(x0 + y, y0 + x, x0 - y, y0 + x, c);
        SSD1306_DrawLine(x0 + y, y0 - x, x0 - y, y0 - x, c);
    }
}
//Функция включения дисплея
void SSD1306_ON(void) {
    SSD1306_WRITECOMMAND(0x8D);
    SSD1306_WRITECOMMAND(0x14);
    SSD1306_WRITECOMMAND(0xAF);
}
//Функция выключения дисплея
void SSD1306_OFF(void) {
    SSD1306_WRITECOMMAND(0x8D);
    SSD1306_WRITECOMMAND(0x10);
    SSD1306_WRITECOMMAND(0xAE);
}

//
// Функция инцииализации  I2C как мастера.
//
void I2CMaster_Init(uint16_t I2C_OwnAddress, uint16_t I2CSlave_Address)
{
    EALLOW;
    //
    // Must put I2C into reset before configuring it
    //
    I2caRegs.I2CMDR.all &= ~(0x20U);

    //
    // I2C configuration. Use a 400kHz I2CCLK with a 50% duty cycle.
    //
    //I2C_initMaster(base, DEVICE_SYSCLK_FREQ, 400000, I2C_DUTYCYCLE_50);
    I2caRegs.I2CPSC.all = 0xB;        // Prescaler - need 7-12 Mhz on module clk
    I2caRegs.I2CCLKL = 0x7;          // NOTE: must be non zero
    I2caRegs.I2CCLKH = 0x8;           // NOTE: must be non zero


    //
    // Configure Master as a Transmitter
    //
    I2caRegs.I2CMDR.bit.MST = 0x1;
    I2caRegs.I2CMDR.bit.TRX = 0x1;

    //
    // Set data count
    //
    I2caRegs.I2CCNT = I2C_NUMBYTES;

    //
    // Set the bit count to 8 bits per data byte
    //
    I2caRegs.I2CMDR.bit.BC = 0x0U;

    //
    // Configure slave and own address
    //
    I2caRegs.I2COAR.all = I2C_OwnAddress;      // Own address
    I2caRegs.I2CSAR.all = I2CSlave_Address;      // Slave address

    //
    // Set emulation mode to FREE
    //
    I2caRegs.I2CMDR.bit.FREE = 0x1;

    //
    //Clear all status
    //
    I2caRegs.I2CSTR.all = 0xFFFF;

    //
    // Enable I2C Interrupts- RRDY
    //
    I2caRegs.I2CIER.all = 0x08;

    //
    // Take I2C out of reset
    //
    I2caRegs.I2CMDR.all |= 0x0020;


}

//
// Функция передачи сообщений по I2C на слэйва.
//
void I2CWrite(uint16_t slaveAddr, uint16_t byteCount, bool sendStopCondition) {
    int i ;
      I2caRegs.I2CSAR.all = slaveAddr;
      I2caRegs.I2CMDR.bit.MST = 0x1;
      I2caRegs.I2CMDR.bit.TRX = 0x1;
      I2caRegs.I2CCNT = byteCount;
      I2caRegs.I2CMDR.bit.STT = 0x1;

      for ( i = 0; i < byteCount; i++) {
          I2caRegs.I2CDXR.all = I2C_TXdata[i];
          while (I2caRegs.I2CSTR.bit.BYTESENT != 0x1);
          I2caRegs.I2CSTR.bit.BYTESENT = 0x1;
      }

      if (sendStopCondition) {
          I2caRegs.I2CMDR.bit.STP = 0x1;
          while (I2caRegs.I2CMDR.bit.STP != 0x0);
          I2caRegs.I2CSTR.bit.BYTESENT = 0x1;
      }
  }
//
// Функция чтения сообщений по I2C слэйва.
//
uint16_t I2CRead(uint16_t slaveAddr, uint16_t byteCount, bool sendStopCondition)
{
    //
    // Configure slave address
    //
    I2caRegs.I2CSAR.all = slaveAddr;

    //
    // Configure I2C in Master Receiver mode
    //
    I2caRegs.I2CMDR.bit.MST = 0x1;
    I2caRegs.I2CMDR.bit.TRX = 0x0;

    //
    //Set Data Count
    //
    //I2caRegs.I2CCNT = byteCount;

    //
    // send Start condition
    //
    I2caRegs.I2CMDR.bit.STT = 0x1;

    uint16_t count = 0;

    //
    // Read the received data into RX buffer
    //
    while(count < I2C_NUMBYTES)
    {
        if(I2caRegs.I2CSTR.bit.RRDY ==0x1)
        {
            I2C_RXdata[count] = I2caRegs.I2CDRR.all;
            count++;
        }
    }

    //
    // Send STOP condition
    //
    if(sendStopCondition)
    {
        I2caRegs.I2CMDR.bit.STP = 0x1;
        while(I2caRegs.I2CMDR.bit.STP != 0x0);

        I2caRegs.I2CSTR.bit.BYTESENT = 0x1;

    }
    return count;
}