/*
 * draw_ssd1306.c
 * begin 20190525 true
 *
 * mostly implemented by true
 * some functions shamelessly copied and modified from interwebs
 *
 * todo:
 * - new MCU is fast enough to convert entire framebuffer quickly.
 *   it may save time and power to render a standard pixel framebuffer,
 *   then convert. would also make code much cleaner. need to test.
 */

#include "draw_ssd1306.h"

#include "../misc/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.
 */
__HIGH_CODE
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;

    uint16_t idx;

    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;
                        idx = nx - startx + dxoff;
                        if (idx >= 512) return;
                        d[idx] |= (1 << dyoff);
                    }
                }
            }
        }
    }
}

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

    uint8_t  d;

    uint8_t  w;

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

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

    uint16_t bytes; // bytes to draw (calculated)

    uint8_t *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? ...)
 */
__HIGH_CODE
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)
{
    uint16_t i;

    uint8_t  d;

    uint8_t  xp;
    uint16_t yp;
    int16_t  pos;

    uint8_t  rh;
    int8_t   yoff;

    int8_t   y_init, yoff_init;

    uint8_t *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));
                    }
                }
            }
        }
    }
}

__HIGH_CODE
void ssd1306fb_draw_pix(uint8_t x, uint8_t y)
{
    uint8_t *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;
    }
}

__HIGH_CODE
void ssd1306fb_draw_hline(uint8_t x1, uint8_t x2, uint8_t y)
{
    uint8_t t;
    uint8_t *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;
    }
}

__HIGH_CODE
void ssd1306fb_draw_vline(uint8_t x, uint8_t y1, uint8_t y2)
{
    uint8_t t;
    uint8_t draw;
    uint8_t *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;
        }
    }
}

__HIGH_CODE
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;
        }
    }
}

__HIGH_CODE
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
__HIGH_CODE
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);
}

__HIGH_CODE
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.
 */
__HIGH_CODE
uint8_t ssd1306fb_get_str_width(const uint8_t *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 *font)
{
    return font[FONT_HEIGHT_POS];
}

__HIGH_CODE
void ssd1306fb_internal_str(const uint8_t *font, const char *str, uint8_t len, int8_t spacing)
{
    const uint8_t 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 i;

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

    uint8_t *jtdata;    // font jump table data for character
    uint16_t 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 = (uint8_t *)(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 *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));
}
