/*
 * fblib.c
 *
 * Adapted by Florian Klemenz for use in the fb_dash project
 *
 * Source and all credit goes to 
 * http://raspberrycompote.blogspot.ie/2014/03/low-level-graphics-on-raspberry-pi-part_14.html
 *
 * Original work by J-P Rosti (a.k.a -rst- and 'Raspberry Compote')
 *
 * Licensed under the Creative Commons Attribution 3.0 Unported License
 * (http://creativecommons.org/licenses/by/3.0/deed.en_US)
 *
 * Distributed in the hope that this will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 *
 */

#include "fblib.h"


// helper function to 'plot' a pixel in given color
void put_pixel(char *fbp, int x, int y, int r, int g, int b)
{
    // calculate the pixel's byte offset inside the buffer
    // note: x * 2 as every pixel is 2 consecutive bytes
    //unsigned int pix_offset = x * 2 + y * finfo.line_length;
    unsigned int pix_offset = x * 2 + y * (WIDTH * 2);

    // now this is about the same as 'fbp[pix_offset] = value'
    // but a bit more complicated for RGB565
    //unsigned short c = ((r / 8) << 11) + ((g / 4) << 5) + (b / 8);
    unsigned short c = ((r / 8) * 2048) + ((g / 4) * 32) + (b / 8);
    // write 'two bytes at once'
    *((unsigned short*)(fbp + pix_offset)) = c;

}

// helper function to draw a rectangle in given color
void fill_rect(char *fbp, int x, int y, int w, int h, int r, int g, int b) {
    int cx, cy;
    for (cy = 0; cy < h; cy++) {
        for (cx = 0; cx < w; cx++) {
           put_pixel(fbp, x + cx, y + cy, r, g, b);
        }
    }
}

// helper function to draw a rectangle outline in given color
void draw_rect(char *fbp, int x0, int y0, int w, int h, int r, int g, int b) {
    draw_line(fbp, x0, y0, x0 + w, y0, r, g, b); // top
    draw_line(fbp, x0, y0, x0, y0 + h, r, g, b); // left
    draw_line(fbp, x0, y0 + h, x0 + w, y0 + h, r, g, b); // bottom
    draw_line(fbp, x0 + w, y0, x0 + w, y0 + h, r, g, b); // right
}

// helper function to draw a circle outline in given color
// (uses Bresenham's circle algorithm)
void draw_circle(char *fbp, int x0, int y0, int radius, int r, int g, int b)
{
    int x = radius;
    int y = 0;
    int radiusError = 1 - x;

    while(x >= y)
    {
        // top left
        put_pixel(fbp, -y + x0, -x + y0, r, g, b);
        // top right
        put_pixel(fbp, y + x0, -x + y0, r, g, b);
        // upper middle left
        put_pixel(fbp, -x + x0, -y + y0, r, g, b);
        // upper middle right
        put_pixel(fbp, x + x0, -y + y0, r, g, b);
        // lower middle left
        put_pixel(fbp, -x + x0, y + y0, r, g, b);
        // lower middle right
        put_pixel(fbp, x + x0, y + y0, r, g, b);
        // bottom left
        put_pixel(fbp, -y + x0, x + y0, r, g, b);
        // bottom right
        put_pixel(fbp, y + x0, x + y0, r, g, b);

        y++;
        if (radiusError < 0)
        {
            radiusError += 2 * y + 1;
        } else {
            x--;
            radiusError+= 2 * (y - x + 1);
        }
    }
}

// helper function to draw a filled circle in given color
// (uses Bresenham's circle algorithm)
void fill_circle(char *fbp, int x0, int y0, int radius, int r, int g, int b) {
    int x = radius;
    int y = 0;
    int radiusError = 1 - x;

    while(x >= y)
    {
        // top
        draw_line(fbp, -y + x0, -x + y0, y + x0, -x + y0, r, g, b);
        // upper middle
        draw_line(fbp, -x + x0, -y + y0, x + x0, -y + y0, r, g, b);
        // lower middle
        draw_line(fbp, -x + x0, y + y0, x + x0, y + y0, r, g, b);
        // bottom 
        draw_line(fbp, -y + x0, x + y0, y + x0, x + y0, r, g, b);

        y++;
        if (radiusError < 0)
        {
            radiusError += 2 * y + 1;
        } else {
            x--;
            radiusError+= 2 * (y - x + 1);
        }
    }
}

// helper function to clear the screen - fill whole 
// screen with given color
void clear_screen(char *fbp) {
   //memset(fbp, 0, vinfo.xres * vinfo.yres * 2); // 16Bit = 8Bit * 2
   memset(fbp, 0, WIDTH * HEIGHT * 2); // 16Bit = 8Bit * 2
}

// helper function to draw a line in given color
// (uses Bresenham's line algorithm)
void draw_line(char *fbp, int x0, int y0, int x1, int y1, int r, int g, int b) {
    int dx = x1 - x0;
    dx = (dx >= 0) ? dx : -dx; // abs()
    int dy = y1 - y0;
    dy = (dy >= 0) ? dy : -dy; // abs()
    int sx;
    int sy;
    if (x0 < x1)
        sx = 1;
    else
        sx = -1;
    if (y0 < y1)
        sy = 1;
    else
        sy = -1;
    int err = dx - dy;
    int e2;
    int done = 0;
    while (!done) {
        put_pixel(fbp, x0, y0, r, g, b);
        if ((x0 == x1) && (y0 == y1))
            done = 1;
        else {
            e2 = 2 * err;
            if (e2 > -dy) {
                err = err - dy;
                x0 = x0 + sx;
            }
            if (e2 < dx) {
                err = err + dx;
                y0 = y0 + sy;
            }
        }
    }
}