/*
 * $Id: draw_ssd1306.c 464 2019-08-01 02:15:54Z true $
 * begin 20190525 true
 *
 * mostly implemented by true
 * some functions shamelessly copied and modified from interwebs
 */

#include "disp/draw_ssd1306.h"

#include "math/sin7.h"

#include <string.h>
#include <stdlib.h>


#define _swap_(a, b)	{ t = a; a = b; b = t; }
#define _min_(a, b)		(((a)<(b))?(a):(b))
#define _max_(a, b)		(((a)>(b))?(a):(b))


SSD1306 *o = &oled;		// we only ever support one...


void ssd1306fb_set_target(SSD1306 *target)
{
	o = target;
}


void ssd1306fb_set_cursor(int8_t x, int8_t y)
{
	o->cursor_x = x;
	o->cursor_y = y;
}

void ssd1306fb_set_color(uint8_t color)
{
	o->state &= ~SSD1306_STATE_PIXEL_MASK;
	o->state |= color & SSD1306_STATE_PIXEL_MASK;
}

/*
 * this function rotatecopies one framebuffer into another,
 * around an axis at the center of the framebuffer.
 *
 * there are assumptions about pages as well; Y pages will always
 * start on a boundary.
 */
void ssd1306fb_rotate(SSD1306 *src, SSD1306 *dst, int8_t rot)
{
	int8_t x, y, nx, ny;
	int8_t startx, starty, endx, endy;

	int8_t yoff;    // page, using same terminology as the rest of the code
	int8_t xoff;    // x offset within the page
	int8_t dxoff;   // destination's x offset
	int8_t dyoff;   // destination's x offset

	uint8_t *s;
	uint8_t *d;

	s = src->fb;
	d = dst->fb;

	endx = dst->width >> 1;
	startx = 0 - endx;
	if ((dst->width & 1) == 0) startx++;

	endy = dst->height >> 1;
	starty = 0 - endy;
	if ((dst->height & 1) == 0) starty++;

	for (y = starty; y <= endy; y++) {
		yoff = (y - starty) & 7;
		xoff = ((y - starty) >> 3) * dst->width;

		for (x = startx; x <= endx; x++) {
			if (s[x - startx + xoff] & (1 << yoff)) {
				nx = (int16_t)(x * cos7(rot) - y * sin7(rot)) >> 7;
				ny = (int16_t)(x * sin7(rot) + y * cos7(rot)) >> 7;

				if ((nx >= startx) && (nx <= endx)) {
					if ((ny >= starty) && (ny <= endy)) {
						dyoff = (ny - starty) & 7;
						dxoff = ((ny - starty) >> 3) * dst->width;
						d[nx - startx + dxoff] |= (1 << dyoff);
					}
				}
			}
		}
	}
}

/*
 * draws from data stored in horizontal priority page byte order
 * (useful for copying between buffers)
 */
void ssd1306fb_copy(int8_t x, int8_t y, uint8_t width, uint8_t height, const uint8_t *dat) compact
{
	uint16_t data  i;

	uint8_t  data  d;

	uint8_t  data  w;

	uint8_t        xp;		// actual pixel x
	uint16_t       yp;		// actual page  y
	int16_t        pos;		// output buffer offset

	uint8_t  data  rh;		// pages to draw
	int8_t   data  yoff;	// y pixel drawing offset

	uint16_t       bytes;	// bytes to draw (calculated)

	uint8_t xdata *fb;
	int16_t        fb_siz;


	if (x >= o->width) return;
	if (y >= o->height) return;

	fb = o->fb;
	fb_siz = (o->height >> 3) * o->width;

	rh = 1 + ((height - 1) >> 3);
	yoff = y & 7;

	bytes = (width * rh);

	w = width;

	height = x;
	width = 0;

	for (i = 0; i < bytes; i++) {
		d = dat[i];

		xp = x + (i % w);
		yp = ((y >> 3) + (i / w)) * o->width;

		pos = xp + yp;

		// half-width
		if (o->state & SSD1306_STATE_STR_HALFWIDTH) {
			if (xp != height) {
				height = xp;
				width++;

				if (width & 0x01) {
					i += rh - 1;
					continue;
				}
			}
		}

		pos -= ((width + 1) >> 1);

		if ((pos < fb_siz) && (xp < o->width)) {
			if (yoff >= 0) {
				if (pos >= 0) {
					if        (o->state & SSD1306_STATE_SET_PIXEL) {
						fb[pos] |=  (d << yoff);
					} else if (o->state & SSD1306_STATE_INVERT_PIXEL) {
						fb[pos] ^=  (d << yoff);
					} else {
						fb[pos] &= ~(d << yoff);
					}
				}

				if (yoff && ((pos + o->width) < fb_siz)) {
					if        (o->state & SSD1306_STATE_SET_PIXEL) {
						fb[pos + o->width] |=  (d >> (8 - yoff));
					} else if (o->state & SSD1306_STATE_INVERT_PIXEL) {
						fb[pos + o->width] ^=  (d >> (8 - yoff));
					} else {
						fb[pos + o->width] &= ~(d >> (8 - yoff));
					}
				}
			}
		}
	}
}

/*
 * draws from data stored in vertical priority page byte order (why? why not just horizontal? ...)
 */
void ssd1306fb_draw_raw(int8_t x, int8_t y, uint8_t width, uint8_t height, const uint8_t *dat, uint16_t dat_offset, uint16_t bytes) compact
{
	uint16_t data  i;

	uint8_t  data  d;

	uint8_t        xp;
	uint16_t       yp;
	int16_t        pos;

	uint8_t  data  rh;
	int8_t   data  yoff;

	int8_t         y_init, yoff_init;

	uint8_t xdata *fb;
	int16_t        fb_siz;


	if (x >= o->width) return;
	if (y >= o->height) return;

	fb = o->fb;
	fb_siz = (o->height >> 3) * o->width;

	rh = 1 + ((height - 1) >> 3);
	yoff = y & 7;

	bytes = (bytes == 0) ? width * rh : bytes;

	y_init = y;
	yoff_init = yoff;

	height = x;
	width = 0;

	for (i = 0; i < bytes; i++) {
		// reset y if next horizontal drawing phase is started
		if (i & rh == 0) {
			y = y_init;
			yoff = yoff_init;
		}

		d = dat[dat_offset + i];

		xp = x + (i / rh);
		yp = ((y >> 3) + (i % rh)) * o->width;

		pos = xp + yp;

		// half-width
		if (o->state & SSD1306_STATE_STR_HALFWIDTH) {
			if (xp != height) {
				height = xp;
				width++;

				if (width & 0x01) {
					i += rh - 1;
					continue;
				}
			}
		}

		pos -= ((width + 1) >> 1);

		if ((pos < fb_siz) && (xp < o->width)) {
			if (yoff >= 0) {
				if (pos >= 0) {
					if        (o->state & SSD1306_STATE_SET_PIXEL) {
						fb[pos] |=  (d << yoff);
					} else if (o->state & SSD1306_STATE_INVERT_PIXEL) {
						fb[pos] ^=  (d << yoff);
					} else {
						fb[pos] &= ~(d << yoff);
					}
				}

				if (yoff && ((pos + o->width) < fb_siz)) {
					if        (o->state & SSD1306_STATE_SET_PIXEL) {
						fb[pos + o->width] |=  (d >> (8 - yoff));
					} else if (o->state & SSD1306_STATE_INVERT_PIXEL) {
						fb[pos + o->width] ^=  (d >> (8 - yoff));
					} else {
						fb[pos + o->width] &= ~(d >> (8 - yoff));
					}
				}
			}
		}
	}
}

void ssd1306fb_draw_pix(uint8_t x, uint8_t y)
{
	uint8_t xdata *fb;

	if (x >= o->width) return;
	if (y >= o->height) return;

	fb = o->fb;
	fb += ((y >> 3) * o->width);
	fb += x;

	y = 1 << (y & 7);

	if        (o->state & SSD1306_STATE_SET_PIXEL) {
		*fb |=  y;
	} else if (o->state & SSD1306_STATE_INVERT_PIXEL) {
		*fb ^=  y;
	} else {
		*fb &= ~y;
	}
}

void ssd1306fb_draw_hline(uint8_t x1, uint8_t x2, uint8_t y)
{
	uint8_t t;
	uint8_t xdata *fb;

	if (x1 >= o->width) return;
	if (x2 >= o->width) return;
	if (y >= o->height) return;

	if (x1 > x2) {
		_swap_(x1, x2);
	}

	fb = o->fb;
	fb += ((y >> 3) * o->width);
	fb += x1;

	y = 1 << (y & 7);
	t = (x2 - x1) + 1;

	if        (o->state & SSD1306_STATE_SET_PIXEL) {
		while (t--) *fb++ |= y;
	} else if (o->state & SSD1306_STATE_INVERT_PIXEL) {
		while (t--) *fb++ ^= y;
	} else {
		y = ~y;
		while (t--) *fb++ &= y;
	}
}

void ssd1306fb_draw_vline(uint8_t x, uint8_t y1, uint8_t y2)
{
	uint8_t t;
	uint8_t draw;
	uint8_t xdata *fb;
	int8_t  yoff, len;

	if (x >= o->width) return;
	if (y1 >= o->height) return;
	if (y2 >= o->height) return;

	if (y1 > y2) {
		_swap_(y1, y2);
	}

	yoff = y1 & 7;

	fb = o->fb;
	fb += ((y1 >> 3) * o->width);
	fb += x;

	len = (y2 - y1) + 1;

	// work the first page if it has an offset
	if (yoff) {
		yoff = 8 - yoff;

		draw = ~(0xff >> yoff);

		if (len < yoff) {
			draw &= (0xff >> (yoff - len));
		}

		if        (o->state & SSD1306_STATE_SET_PIXEL) {
			*fb |=  draw;
		} else if (o->state & SSD1306_STATE_INVERT_PIXEL) {
			*fb ^=  draw;
		} else {
			*fb &= ~draw;
		}

		if (len < yoff) return;

		len -= yoff;
		fb += o->width;
	}

	// work any full pages
	while (len >= 8) {
		if        (o->state & SSD1306_STATE_SET_PIXEL) {
			*fb = 0xff;
		} else if (o->state & SSD1306_STATE_INVERT_PIXEL) {
			*fb ^= ~(*fb);
		} else {
			*fb = 0x00;
		}

		len -= 8;
		fb += o->width;
	}

	// work last page
	if (len > 0) {
		draw = (1 << (len & 7)) - 1;

		if        (o->state & SSD1306_STATE_SET_PIXEL) {
			*fb |=  draw;
		} else if (o->state & SSD1306_STATE_INVERT_PIXEL) {
			*fb ^=  draw;
		} else {
			*fb &= ~draw;
		}
	}
}

void ssd1306fb_draw_line(uint8_t x1, uint8_t y1, uint8_t x2, uint8_t y2)
{
	int8_t t;
	int8_t dx, dy;
	int8_t err;
	int8_t ystep;

	int8_t steep = abs(y2 - y1) > abs(x2 - x1);

	if (steep) {
		_swap_(x1, y1);
	    _swap_(x2, y2);
	}

	if (x1 > x2) {
		_swap_(x1, x2);
		_swap_(y1, y2);
	}

	dx = x2 - x1;
	dy = abs(y2 - y1);

	err = dx / 2;

	if (y1 < y2) {
		ystep = 1;
	} else {
		ystep = -1;
	}

	for (; x1 <= x2; x1++) {
		if (steep) {
			ssd1306fb_draw_pix(y1, x1);
		} else {
			ssd1306fb_draw_pix(x1, y1);
		}

		err -= dy;

		if (err < 0) {
			y1 += ystep;
			err += dx;
		}
	}
}

void ssd1306fb_draw_rect(uint8_t x1, uint8_t y1, uint8_t x2, uint8_t y2)
{
	ssd1306fb_draw_vline(x1, y1 + 1, y2 - 1);
	ssd1306fb_draw_hline(x1, x2, y1);
	ssd1306fb_draw_hline(x1, x2, y2);
	ssd1306fb_draw_vline(x2, y1 + 1, y2 - 1);
}


// note: rect fill not working right now, lol
void ssd1306fb_draw_rect_fill(uint8_t x1, uint8_t y1, uint8_t x2, uint8_t y2)
{
	uint8_t t;

	if (y1 > y2) _swap_(y1, y2);

	do {
		// todo: replace with vline implementation, it might be faster,
		// as multiple bits are set per assign vs one bit max with hline
		ssd1306fb_draw_hline(x1, x2, y1);
	} while (y1++ < y2);
}

void ssd1306fb_draw_circle(int8_t x, int8_t y, uint8_t radius)
{
	int8_t xs = 0;
	int8_t ys = radius;
	int8_t dp = 1 - radius;

	do {
		if (dp < 0) {
			dp = dp + 2 * (xs++) + 3;
		} else {
			dp = dp + 2 * (xs++) - 2 * (ys--) + 5;
		}

		ssd1306fb_draw_pix(x + xs, y + ys);
		ssd1306fb_draw_pix(x - xs, y + ys);
		ssd1306fb_draw_pix(x + xs, y - ys);
		ssd1306fb_draw_pix(x - xs, y - ys);
		ssd1306fb_draw_pix(x + ys, y + xs);
		ssd1306fb_draw_pix(x - ys, y + xs);
		ssd1306fb_draw_pix(x + ys, y - xs);
		ssd1306fb_draw_pix(x - ys, y - xs);
	} while (xs < ys);

	ssd1306fb_draw_pix(x + radius, y);
	ssd1306fb_draw_pix(x, y + radius);
	ssd1306fb_draw_pix(x - radius, y);
	ssd1306fb_draw_pix(x, y - radius);
}

/*
 * character string drawing functions draw at the cursor position.
 */
uint8_t ssd1306fb_get_str_width(const uint8_t code *font, const char *str, uint8_t len, int8_t extra_spacing)
{
	uint8_t first = font[FONT_FIRST_CHAR_POS];
	uint8_t chr_width;
	uint8_t str_width = 0;
	uint8_t w;

	w = len;

	while (w--) {
		chr_width = font[FONT_JUMPTABLE_START + (str[w] - first) * FONT_JUMPTABLE_BYTES + FONT_JUMPTABLE_WIDTH];

		if (o->state & SSD1306_STATE_STR_HALFWIDTH) {
			chr_width >>= 1;
		}

		str_width += chr_width;
	}

	str_width += extra_spacing * (len - 1);

	return str_width;
}

uint8_t ssd1306fb_get_font_height(const uint8_t code *font)
{
	return font[FONT_HEIGHT_POS];
}

void ssd1306fb_internal_str(const uint8_t code *font, const char *str, uint8_t len, int8_t spacing) compact
{
	const uint8_t data height	= font[FONT_HEIGHT_POS];
	const uint8_t first			= font[FONT_FIRST_CHAR_POS];
	const uint16_t jt_siz		= font[FONT_CHAR_NUM_POS] * FONT_JUMPTABLE_BYTES;
	const uint16_t last			= font[FONT_CHAR_NUM_POS] + first;

	uint8_t data i;

	uint8_t c;				// current character
	uint8_t offset;			// offset from first character in font table (usually space 0x20)

	uint8_t code *jtdata;	// font jump table data for character
	uint16_t data pos;		// position of actual character data

	if (!len) return;

	for (i = 0; i < len; i++) {
		if (o->cursor_x > o->width) return;

		c = str[i];

		// character valid?
		if (c >= first && c < last) {
			offset = c - first;

			// get
			jtdata = (font + FONT_JUMPTABLE_START + (offset * FONT_JUMPTABLE_BYTES));

			if (!(jtdata[FONT_JUMPTABLE_MSB] == 255 && jtdata[FONT_JUMPTABLE_LSB] == 255)) {
				pos = FONT_JUMPTABLE_START +
						jt_siz +
						((jtdata[FONT_JUMPTABLE_MSB] << 8) +
						jtdata[FONT_JUMPTABLE_LSB]);

				ssd1306fb_draw_raw(
						o->cursor_x,
						o->cursor_y,
						jtdata[FONT_JUMPTABLE_WIDTH],
						height,
						font,
						pos,
						jtdata[FONT_JUMPTABLE_SIZE]);
			}

			offset = jtdata[FONT_JUMPTABLE_WIDTH];

			if (o->state & SSD1306_STATE_STR_HALFWIDTH) {
				offset >>= 1;
			}

			o->cursor_x += offset;

			if (i + 1 != len) {
				o->cursor_x += spacing;
			}
		}
	}
}

void ssd1306fb_draw_str(const uint8_t code *font, const char *str, int8_t extra_spacing)
{
	uint8_t lh;
	uint8_t len;

	lh = *(font + FONT_HEIGHT_POS);
	len = strlen(str);

	ssd1306fb_internal_str(font, str, len, extra_spacing); //, ssd1306fb_get_str_width(font, str, len));
}
