#include "main.h"
#include "display_gfx.h"

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;

/**
 * @brief Draw one pixel in a buffer.
 *
 * @param disp  A pointer to display struct
 * @param x     Top-most x coordinate
 * @param y     Top-most y coordinate
 * @param color Color of pixel WHITE(0), BLACK(1) or INVERSE(2)
 */
void DISP_drawPixel(GFX_display_t *disp, uint8_t x, uint8_t y, GFX_Color_t color)
{
	if (x > disp->width || y > disp->height)
		return;

	switch (color)
	{
	case GFX_WHITE:
		disp->buffor[(y / 8) * disp->width + x] |= (1 << (y % 8));
		break;
	case GFX_BLACK:
		disp->buffor[(y / 8) * disp->width + x] &= ~(1 << (y % 8));
		break;
	case GFX_INVERSE:
		disp->buffor[(y / 8) * disp->width + x] ^= (1 << (y % 8));
		break;
	default:
		break;
	}
}
void DISP_drawBitmapSlow(GFX_display_t *disp, const GFX_bitmap_t *bitmap, uint8_t x, uint8_t y)
{
	uint8_t row_div_by_8 = 0;
	uint8_t mask = 0;
	uint8_t bitmap_byte = 0;

	for (uint8_t row = 0; row < bitmap->height; row++)
	{
		row_div_by_8 = row / 8;
		mask = (row % 8);
		for (uint8_t col = 0; col < bitmap->width; col++)
		{

			bitmap_byte = bitmap->bitmap[row_div_by_8 * bitmap->width + col];
			if (bitmap_byte & (1 << mask))
			{
				DISP_drawPixel(disp, col + x, row + y, GFX_WHITE);
			}
			else
			{
				DISP_drawPixel(disp, col + x, row + y, GFX_BLACK);
			}
		}
	}
}

/**
 * @brief Draw a vertical line.
 *
 * @param disp  A pointer to display struct
 * @param x0    Start point x coordinate
 * @param y0    Start point y coordinate
 * @param x1    End point x coordinate
 * @param y1    End point y coordinate
 * @param color Color of pixel WHITE(0), BLACK(1) or INVERSE(2)
 */
void DISP_drawSlashLine(GFX_display_t *disp, 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)
		{
			DISP_drawPixel(disp, y0, x0, color);
		}
		else
		{
			DISP_drawPixel(disp, x0, y0, color);
		}
		err -= dy;
		if (err < 0)
		{
			err += dx;
			y0 += step;
		}
	}
}

/**
 * @brief  Write a perfectly vertical line
 * @param  disp  	A pointer to display struct
 * @param  x			Left-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 DISP_drawVerticalLine(GFX_display_t *disp, int16_t x, int16_t y, int16_t height, GFX_Color_t color)
{
	for (int16_t i = y; i < y + height; i++)
	{
		DISP_drawPixel(disp, x, i, color);
	}
}

/**
  @brief  Write a perfectly horizontal line
  @param  disp 	A pointer to display struct
  @param  x		Left-most x coordinate
  @param  y		Top-most y coordinate
  @param  width Width in pixels
  @param  color Color of pixel WHITE(0), BLACK(1) or INVERSE(2)
*/
void DISP_drawHorizontalLine(GFX_display_t *disp, uint8_t x, uint8_t y, uint8_t width, GFX_Color_t color)
{
	for (int16_t i = x; i < x + width; i++)
	{
		DISP_drawPixel(disp, i, y, color);
	}
}

/**
 * @brief Draw a rectangle with no fill color
 * @param disp 		A pointer to display struct
 * @param x			Top left corner x coordinate
 * @param y			Top left corner y coordinate
 * @param width		Width in pixels
 * @param height	Height in pixels
 * @param radius	Radius of corner rounding
 * @param color		Color of pixel WHITE(0), BLACK(1) or INVERSE(2)
 */
void DISP_drawRect(GFX_display_t *disp, int16_t x, int16_t y, int16_t width, int16_t height, GFX_Color_t color)
{
	DISP_drawHorizontalLine(disp, x, y, width, color);
	DISP_drawHorizontalLine(disp, x, y + height - 1, width, color);
	DISP_drawVerticalLine(disp, x, y, height, color);
	DISP_drawVerticalLine(disp, x + width - 1, y, height, color);
}

/*!
	@brief Draw a circle outline
	@param disp		A pointer to display struct
	@param x0		Center-point x coordinate
	@param y0		Center-point y coordinate
	@param radius	Radius of circle
	@param color	Color of pixel WHITE(0), BLACK(1) or INVERSE(2)
*/
void DISP_drawCircle(GFX_display_t *disp, 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;

	DISP_drawPixel(disp, x0, y0 + radius, color);
	DISP_drawPixel(disp, x0, y0 - radius, color);
	DISP_drawPixel(disp, x0 + radius, y0, color);
	DISP_drawPixel(disp, x0 - radius, y0, color);

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

		DISP_drawPixel(disp, x0 + x, y0 + y, color);
		DISP_drawPixel(disp, x0 - x, y0 + y, color);
		DISP_drawPixel(disp, x0 + x, y0 - y, color);
		DISP_drawPixel(disp, x0 - x, y0 - y, color);
		DISP_drawPixel(disp, x0 + y, y0 + x, color);
		DISP_drawPixel(disp, x0 - y, y0 + x, color);
		DISP_drawPixel(disp, x0 + y, y0 - x, color);
		DISP_drawPixel(disp, x0 - y, y0 - x, color);
	}
}

/*!
	@brief Quarter-circle drawer, used to do circles and roundrects
	@param disp  	A pointer to display struct
	@param x0		Center-point x coordinate
	@param y0		Center-point y coordinate
	@param radius	Radius of circle
	@param corner	Mask bit to indicate which quarters of the circle we're doing
	@param color 	Color of pixel WHITE(0), BLACK(1) or INVERSE(2)
*/
void DISP_drawQuarterCircle(GFX_display_t *disp, int16_t x0, int16_t y0, uint8_t radius, GFX_CircCorners_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)
		{
			DISP_drawPixel(disp, x0 + x, y0 + y, color);
			DISP_drawPixel(disp, x0 + y, y0 + x, color);
		}
		if (corner & BOTTOM_RIGHT)
		{
			DISP_drawPixel(disp, x0 + x, y0 - y, color);
			DISP_drawPixel(disp, x0 + y, y0 - x, color);
		}
		if (corner & TOP_LEFT)
		{
			DISP_drawPixel(disp, x0 - y, y0 + x, color);
			DISP_drawPixel(disp, x0 - x, y0 + y, color);
		}
		if (corner & TOP_RIGHT)
		{
			DISP_drawPixel(disp, x0 - y, y0 - x, color);
			DISP_drawPixel(disp, x0 - x, y0 - y, color);
		}
	}
}

/**
 * @brief Draw a rounded rectangle with no fill color
 * @param disp 		A pointer to display struct
 * @param x			Top left corner x coordinate
 * @param y			Top left corner y coordinate
 * @param width		Width in pixels
 * @param height	Height in pixels
 * @param radius	Radius of corner rounding
 * @param color		Color of pixel WHITE(0), BLACK(1) or INVERSE(2)
 */
void DISP_drawRoundRect(GFX_display_t *disp, int16_t x, int16_t y, int16_t width, int16_t height, int16_t radius, GFX_Color_t color)
{
	int16_t max_radius = ((width < height) ? width : height) / 2; // 1/2 minor axis
	if (radius > max_radius)
		radius = max_radius;
	// smarter version
	DISP_drawHorizontalLine(disp, x + radius, y, width - 2 * radius, color);			  // Top
	DISP_drawHorizontalLine(disp, x + radius, y + height - 1, width - 2 * radius, color); // Bottom
	DISP_drawVerticalLine(disp, x, y + radius, height - 2 * radius, color);				  // Left
	DISP_drawVerticalLine(disp, x + width - 1, y + radius, height - 2 * radius, color);	  // Right
	// draw four corners
	DISP_drawQuarterCircle(disp, x + radius, y + radius, radius, 1, color);
	DISP_drawQuarterCircle(disp, x + width - radius - 1, y + radius, radius, 2, color);
	DISP_drawQuarterCircle(disp, x + width - radius - 1, y + height - radius - 1, radius, 4, color);
	DISP_drawQuarterCircle(disp, x + radius, y + height - radius - 1, radius, 8, color);
}

static void _getBoundry(GFX_display_t *disp, 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) > disp->width)
	{
		boundry->buf_col_last = disp->width;
	}
	else
	{
		boundry->buf_col_last = bitmap_width + pos_x;
	}

	if (bitmap_height + pos_y > disp->height)
	{
		boundry->buf_row_last = disp->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_BitmapColor_t color)
{
	switch (color)
	{
	case BM_INVERSE:
		return ~(bitmap[index]);
	case BM_FILL_WHITE:
		return 0xFF;
	case BM_FILL_BLACK:
		return 0x00;
	default:
		return bitmap[index];
	}
}

/**
 * @brief Draw a  1-bit image at the specified (x,y) position.
 * @param disp A pointer to display struct
 * @param bitmap byte array with monochrome bitmap
 * @param bitmap_width Width in pixels
 * @param bitmap_height Height in pixels
 * @param pos_x Top left corner x coordinate
 * @param pos_y Top left corner y coordinate
 * @param color Color of pixel BM_NORMAL or BM_INVERSE for bitmap. BM_WHITE or BM_BLACK for fill region.
 */
void DISP_drawBitmap(GFX_display_t *disp, const GFX_bitmap_t *bitmap, int8_t pos_x, int8_t pos_y, GFX_BitmapColor_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(disp, &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, bitmap->width * (b.bitmap_row_first - 1) + b.bitmap_col, color) >> (8 - b.bitmap_shift);
		}
		else
		{
			tmp_buf16 = disp->buffor[b.buf_row_first * disp->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 * disp->width + col;

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

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

void DISP_drawBitmapShift(GFX_display_t *disp, const GFX_bitmap_t *bitmap, int8_t pos_x, int8_t pos_y, uint8_t shift_left, GFX_BitmapColor_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(disp, &b, bitmap->width, bitmap->height, pos_x, pos_y);

	b.bitmap_col = (b.bitmap_col + shift_left) % bitmap->width;
	for (uint8_t col = b.buf_col_first; col < b.buf_col_last; col++, b.bitmap_col = (b.bitmap_col + 1) % bitmap->width)
	{
		tmp_buf16 = 0;
		bitmap_row = b.bitmap_row_first;

		if (b.bitmap_row_first > 0)
		{
			tmp_buf16 = _getBitmapByte(bitmap->bitmap, bitmap->width * (b.bitmap_row_first - 1) + b.bitmap_col, color) >> (8 - b.bitmap_shift);
		}
		else
		{
			tmp_buf16 = disp->buffor[b.buf_row_first * disp->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 * disp->width + col;

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

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

void DISP_drawFillRect(GFX_display_t *disp, int16_t x, int16_t y, int16_t width, int16_t height)
{
	GFX_bitmap_t area = {
		.height = height,
		.width = width,
		.bitmap = NULL,
	};
	DISP_drawBitmap(disp, &area, x, y, BM_FILL_WHITE);
}

void DISP_clearRegion(GFX_display_t *disp, int16_t x, int16_t y, int16_t width, int16_t height)
{
	GFX_bitmap_t area = {
		.height = height,
		.width = width,
		.bitmap = NULL,
	};
	DISP_drawBitmap(disp, &area, x, y, BM_FILL_BLACK);
}

void DISP_clearScreen(GFX_display_t *disp)
{
	GFX_bitmap_t area = {.height = disp->height, .width = disp->width, .bitmap = NULL};
	DISP_drawBitmap(disp, &area, 0, 0, BM_FILL_BLACK);
}