#include "gfx.h"

/**
 * @brief Set the pixel
 *
 * @param hOled Display object
 * @param x X coordinate
 * @param y Y coordinate
 * @param color Color of pixel WHITE(0), BLACK(1) or INVERSE(2)
 */
void writePixel(OLED_HandleTypeDef *hOled, uint8_t x, uint8_t y, GFX_Color_t color)
{
    if (x > hOled->Width || y > hOled->Height)
        return;

    switch (color)
    {
    case WHITE:
        hOled->Buffer[(y / 8) * hOled->Width + x] |= (1 << (y % 8));
        break;
    case BLACK:
        hOled->Buffer[(y / 8) * hOled->Width + x] &= ~(1 << (y % 8));
        break;
    case INVERSE:
        hOled->Buffer[(y / 8) * hOled->Width + x] ^= (1 << (y % 8));
        break;
    default:
        break;
    }
}

typedef struct
{
    uint16_t bitmap_max_idx;
    uint8_t buf_row_first;
    uint8_t buf_row_last;
    uint8_t buf_col_first;
    uint8_t buf_col_last;
    uint8_t buf_mask_top;
    uint8_t buf_mask_bottom;
    uint8_t bitmap_col;
    uint8_t bitmap_row_first;
    uint8_t bitmap_row_last;
    uint8_t bitmap_shift;
} buf_bitmap_boundry_t;

static void _getBoundry(OLED_HandleTypeDef *hOled, buf_bitmap_boundry_t *boundry, uint8_t bitmap_width, uint8_t bitmap_height, int8_t pos_x, int8_t pos_y)
{
    if (pos_x < 0)
    {
        boundry->bitmap_col = pos_x * -1;
        boundry->buf_col_first = 0;
    }
    else
    {
        boundry->bitmap_col = 0;
        boundry->buf_col_first = pos_x;
    }

    if (pos_y < 0)
    {
        boundry->bitmap_shift = 8 + (pos_y % 8);
        boundry->bitmap_row_first = (pos_y / 8) * (-1) + 1;
        boundry->buf_row_first = 0;
        boundry->buf_mask_top = 0;
    }
    else
    {
        boundry->bitmap_shift = pos_y % 8;
        boundry->bitmap_row_first = 0;
        boundry->buf_row_first = pos_y / 8;
        boundry->buf_mask_top = 0xFF >> (8 - boundry->bitmap_shift);
    }
    boundry->buf_mask_bottom = 0xFF << ((pos_y + bitmap_height) % 8);
    if (boundry->buf_mask_bottom == 0xFF)
    {
        boundry->buf_mask_bottom = 0;
    }

    if ((bitmap_width + pos_x) > hOled->Width)
    {
        boundry->buf_col_last = hOled->Width;
    }
    else
    {
        boundry->buf_col_last = bitmap_width + pos_x;
    }

    if (bitmap_height + pos_y > hOled->Height)
    {
        boundry->buf_row_last = hOled->Height / 8;
    }
    else
    {
        boundry->buf_row_last = (bitmap_height + pos_y + 7) / 8;
    }

    boundry->bitmap_row_last = (pos_y + bitmap_height) / 8;
    boundry->bitmap_max_idx = bitmap_width * ((bitmap_height + 7) / 8);
}

static inline uint8_t _getBitmapByte(const uint8_t *bitmap, uint16_t index, GFX_Color_t color)
{
    switch (color)
    {
    case INVERSE:
        return ~(bitmap[index]);
    case WHITE:
        return 0xFF;
    case BLACK:
        return 0x00;
    default:
        return bitmap[index];
    }
}
/**
 * @brief A function that writes a bitmap into the buffer at the given position.
 * 			0,0 -------->x
 * 			 |
 * 			 |
 * 			\ /
 * 			 y
 * @param bitmap A pointer to bitmap array.
 * @param bitmap_width Bitmap witdh in pixels.
 * @param bitmap_height Bitmap height in pixels.
 * @param pos_x Position x in the display
 * @param pos_y Position y in the display
 * @param color NORMAL (2) normal mode or INVERSE(3) mode for bitmap
 * 				WHITE (0) or BLACK (1) for fill screen
 */
void writeBitmap(OLED_HandleTypeDef *hOled, const uint8_t *bitmap, uint8_t bitmap_width, uint8_t bitmap_height, int8_t pos_x, int8_t pos_y, GFX_Color_t color)
{
    if (bitmap_width + pos_x < 0 || bitmap_height + pos_y < 0)
        return;

    uint16_t tmp_buf16, bitmap_idx, buf_idx;
    uint8_t tmp_bitmap, bitmap_row;

    buf_bitmap_boundry_t b;
    _getBoundry(hOled, &b, bitmap_width, bitmap_height, pos_x, pos_y);

    for (uint8_t col = b.buf_col_first; col < b.buf_col_last; col++, b.bitmap_col++)
    {
        tmp_buf16 = 0;
        bitmap_row = b.bitmap_row_first;

        if (b.bitmap_row_first > 0)
        {
            tmp_buf16 = _getBitmapByte(bitmap, bitmap_width * (b.bitmap_row_first - 1) + b.bitmap_col, color) >> (8 - b.bitmap_shift);
        }
        else
        {
            tmp_buf16 = hOled->Buffer[b.buf_row_first * hOled->Width + col] & b.buf_mask_top;
        }

        for (uint8_t buf_row = b.buf_row_first; buf_row < b.buf_row_last; buf_row++, bitmap_row++)
        {
            bitmap_idx = bitmap_width * bitmap_row + b.bitmap_col;
            buf_idx = buf_row * hOled->Width + col;

            if (bitmap_idx < b.bitmap_max_idx)
            {
                tmp_bitmap = _getBitmapByte(bitmap, bitmap_idx, color);
                tmp_buf16 |= tmp_bitmap << b.bitmap_shift;
            }

            if (b.bitmap_row_last == buf_row)
            {
                hOled->Buffer[buf_idx] = (hOled->Buffer[buf_idx] & b.buf_mask_bottom) | (tmp_buf16 & ~(b.buf_mask_bottom));
            }
            else
            {
                hOled->Buffer[buf_idx] = (uint8_t)tmp_buf16;
            }
            tmp_buf16 = tmp_buf16 >> 8;
        }
    }
}

void writeSlashLine(OLED_HandleTypeDef *hOled, int16_t x0, int16_t y0, int16_t x1, int16_t y1, GFX_Color_t color)
{
    uint8_t steep = _diff(y1, y0) > _diff(x1, x0); // bool
    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 = x1 - x0;
    int16_t dy = _diff(y1, y0);
    int16_t err = dx >> 1;
    int16_t step = (y0 < y1) ? 1 : -1;

    for (; x0 <= x1; x0++)
    {
        if (steep)
        {
            writePixel(hOled, y0, x0, color);
        }
        else
        {
            writePixel(hOled, x0, y0, color);
        }
        err -= dy;
        if (err < 0)
        {
            err += dx;
            y0 += step;
        }
    }
}

/**************************************************************************/
/*!
  @brief  Write a perfectly vertical line
  @param  x       Top-most x coordinate
  @param  y       Top-most y coordinate
  @param  height       Height in pixels
  @param  color   Color of pixel WHITE(0), BLACK(1) or INVERSE(2)
*/
/**************************************************************************/
void writeVerticalLine(OLED_HandleTypeDef *hOled, int16_t x, int16_t y, int16_t height, GFX_Color_t color)
{
    for (int16_t i = y; i < y + height; i++)
    {
        writePixel(hOled, x, i, color);
    }
}

/**************************************************************************/
/*!
  @brief  Write a perfectly horizontal line
  @param  x       Left-most x coordinate
  @param  y       Left-most y coordinate
  @param  width       Width in pixels
  @param  color   Color of pixel WHITE(0), BLACK(1) or INVERSE(2)
*/
/**************************************************************************/
void writeHorizontalLine(OLED_HandleTypeDef *hOled, int16_t x, int16_t y, int16_t width, GFX_Color_t color)
{
    for (int16_t i = x; i < x + width; i++)
    {
        writePixel(hOled, i, y, color);
    }
}

void writeRect(OLED_HandleTypeDef *hOled, int16_t x, int16_t y, int16_t width, int16_t height, GFX_Color_t color)
{
    writeHorizontalLine(hOled, x, y, width, color);
    writeHorizontalLine(hOled, x, y + height - 1, width, color);
    writeVerticalLine(hOled, x, y, height, color);
    writeVerticalLine(hOled, x + width - 1, y, height, color);
}

void writeFillRect(OLED_HandleTypeDef *hOled, int16_t x, int16_t y, int16_t width, int16_t height, GFX_Color_t color)
{
    writeBitmap(hOled, NULL, width, height, x, y, color);
}

/**************************************************************************/
/*!
   @brief    Draw a circle outline
    @param    x0   Center-point x coordinate
    @param    y0   Center-point y coordinate
    @param    radius   Radius of circle
    @param    color 16-bit 5-6-5 Color to draw with
*/
/**************************************************************************/
void writeCircle(OLED_HandleTypeDef *hOled, int16_t x0, int16_t y0, uint8_t radius, GFX_Color_t color)
{

    int16_t f = 1 - radius;
    int16_t ddF_x = 1;
    int16_t ddF_y = -2 * radius;
    int16_t x = 0;
    int16_t y = radius;

    writePixel(hOled, x0, y0 + radius, color);
    writePixel(hOled, x0, y0 - radius, color);
    writePixel(hOled, x0 + radius, y0, color);
    writePixel(hOled, x0 - radius, y0, color);

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

        writePixel(hOled, x0 + x, y0 + y, color);
        writePixel(hOled, x0 - x, y0 + y, color);
        writePixel(hOled, x0 + x, y0 - y, color);
        writePixel(hOled, x0 - x, y0 - y, color);
        writePixel(hOled, x0 + y, y0 + x, color);
        writePixel(hOled, x0 - y, y0 + x, color);
        writePixel(hOled, x0 + y, y0 - x, color);
        writePixel(hOled, x0 - y, y0 - x, color);
    }
    endWrite();
}

/**************************************************************************/
/*!
    @brief    Quarter-circle drawer, used to do circles and roundrects
    @param    x0   Center-point x coordinate
    @param    y0   Center-point y coordinate
    @param    radius   Radius of circle
    @param    corner  Mask bit #1 or bit #2 to indicate which quarters of
   the circle we're doing
    @param    color 16-bit 5-6-5 Color to draw with
*/
/**************************************************************************/
void writeQuarterCircle(OLED_HandleTypeDef *hOled, int16_t x0, int16_t y0, uint8_t radius, CIRC_Corners_t corner, GFX_Color_t color)
{
    int16_t f = 1 - radius;
    int16_t ddF_x = 1;
    int16_t ddF_y = -2 * radius;
    int16_t x = 0;
    int16_t y = radius;

    while (x < y)
    {
        if (f >= 0)
        {
            y--;
            ddF_y += 2;
            f += ddF_y;
        }
        x++;
        ddF_x += 2;
        f += ddF_x;
        if (corner & BOTTOM_LEFT)
        {
            writePixel(hOled, x0 + x, y0 + y, color);
            writePixel(hOled, x0 + y, y0 + x, color);
        }
        if (corner & BOTTOM_RIGHT)
        {
            writePixel(hOled, x0 + x, y0 - y, color);
            writePixel(hOled, x0 + y, y0 - x, color);
        }
        if (corner & TOP_LEFT)
        {
            writePixel(hOled, x0 - y, y0 + x, color);
            writePixel(hOled, x0 - x, y0 + y, color);
        }
        if (corner & TOP_RIGHT)
        {
            writePixel(hOled, x0 - y, y0 - x, color);
            writePixel(hOled, x0 - x, y0 - y, color);
        }
    }
}