osd_utils.c

Go to the documentation of this file.

/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, see <http://www.gnu.org/licenses/>
 *
 * Additional note on redistribution: The copyright and license notices above
 * must be maintained in each individual source file that is a derivative work
 * of this source file; otherwise redistribution is prohibited.
 */

#include <openpilot.h>
#include "pios_video.h"
#include "fonts.h"
#include "osd_utils.h"
#include "physical_constants.h"
#include "math.h"
#include "misc_math.h"

#include "gpsposition.h"
#include "homelocation.h"

extern struct FontEntry* fonts[NUM_FONTS];

#if defined(PIOS_VIDEO_SPLITBUFFER)
extern uint8_t *draw_buffer_level;
extern uint8_t *draw_buffer_mask;
extern uint8_t *disp_buffer_level;
extern uint8_t *disp_buffer_mask;
#else
extern uint8_t *draw_buffer;
extern uint8_t *disp_buffer;
#endif /* defined(PIOS_VIDEO_SPLITBUFFER) */


void clearGraphics()
{
#if defined(PIOS_VIDEO_SPLITBUFFER)
        memset((uint8_t *)draw_buffer_mask, 0, BUFFER_HEIGHT * BUFFER_WIDTH);
        memset((uint8_t *)draw_buffer_level, 0, BUFFER_HEIGHT * BUFFER_WIDTH);
#else
        memset((uint8_t *)draw_buffer, 0, BUFFER_HEIGHT * BUFFER_WIDTH);
#endif /* defined(PIOS_VIDEO_SPLITBUFFER) */
}

void draw_image(uint16_t x, uint16_t y, const struct Image * image)
{
#if defined(PIOS_VIDEO_SPLITBUFFER)
        CHECK_COORDS(x + image->width, y + image->height);
        uint8_t byte_width = image->width / 8;
        uint8_t pixel_offset = x % 8;
        uint8_t mask1 = 0xFF;
        uint8_t mask2 = 0x00;

        if (pixel_offset > 0) {
                for (uint8_t i = 0; i<pixel_offset; i++) {
                        mask2 |= 0x01 << i;
                }
                mask1 = ~mask2;
        }

        for (uint16_t yp = 0; yp < image->height; yp++) {
                for (uint16_t xp = 0; xp < image->width / 8; xp++) {
                        draw_buffer_level[(y + yp) * BUFFER_WIDTH + xp + x / 8] |= (image->level[yp * byte_width + xp] & mask1) >> pixel_offset;
                        draw_buffer_mask[(y + yp) * BUFFER_WIDTH + xp + x / 8] |= (image->mask[yp * byte_width + xp] & mask1) >> pixel_offset;
                        if (pixel_offset > 0) {
                                draw_buffer_level[(y + yp) * BUFFER_WIDTH + xp + x / 8 + 1] |= (image->level[yp * byte_width + xp] & mask2) <<
                                                (8 - pixel_offset);
                                draw_buffer_mask[(y + yp) * BUFFER_WIDTH + xp + x / 8 + 1] |= (image->mask[yp * byte_width + xp] & mask2) <<
                                                (8 - pixel_offset);
                        }
                }
        }
#else
        CHECK_COORDS(x + image->width, y + image->height);
        uint8_t byte_width = image->width / 4;
        uint8_t pixel_offset = 2 * (x % 4);
        uint8_t mask1 = 0xFF;
        uint8_t mask2 = 0x00;

        if (pixel_offset > 0) {
                for (uint8_t i = 0; i<pixel_offset; i++) {
                        mask2 |= 0x01 << i;
                }
                mask1 = ~mask2;
        }

        for (uint16_t yp = 0; yp < image->height; yp++) {
                for (uint16_t xp = 0; xp < byte_width; xp++) {
                        draw_buffer[(y + yp) * BUFFER_WIDTH + xp + x / 4] |= (image->data[yp * byte_width + xp] & mask1) >> pixel_offset;
                        if (pixel_offset > 0) {
                                draw_buffer[(y + yp) * BUFFER_WIDTH + xp + x / 4 + 1] |= (image->data[yp * byte_width + xp] & mask2) <<
                                                (8 - pixel_offset);
                        }
                }
        }
#endif /* PIOS_VIDEO_SPLITBUFFER */
}

void plotFourQuadrants(int32_t centerX, int32_t centerY, int32_t deltaX, int32_t deltaY)
{
        write_pixel_lm(centerX + deltaX, centerY + deltaY, 1, 1); // Ist      Quadrant
        write_pixel_lm(centerX - deltaX, centerY + deltaY, 1, 1); // IInd     Quadrant
        write_pixel_lm(centerX - deltaX, centerY - deltaY, 1, 1); // IIIrd    Quadrant
        write_pixel_lm(centerX + deltaX, centerY - deltaY, 1, 1); // IVth     Quadrant
}

void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius)
{
        int64_t doubleHorizontalRadius = horizontalRadius * horizontalRadius;
        int64_t doubleVerticalRadius   = verticalRadius * verticalRadius;

        int64_t error = doubleVerticalRadius - doubleHorizontalRadius * verticalRadius + (doubleVerticalRadius >> 2);

        int x = 0;
        int y = verticalRadius;
        int deltaX = 0;
        int deltaY = (doubleHorizontalRadius << 1) * y;

        plotFourQuadrants(centerX, centerY, x, y);

        while (deltaY >= deltaX) {
                x++;
                deltaX += (doubleVerticalRadius << 1);

                error  += deltaX + doubleVerticalRadius;

                if (error >= 0) {
                        y--;
                        deltaY -= (doubleHorizontalRadius << 1);

                        error  -= deltaY;
                }
                plotFourQuadrants(centerX, centerY, x, y);
        }

        error = (int64_t)(doubleVerticalRadius * (x + 1 / 2.0f) * (x + 1 / 2.0f) + doubleHorizontalRadius * (y - 1) * (y - 1) - doubleHorizontalRadius * doubleVerticalRadius);

        while (y >= 0) {
                error  += doubleHorizontalRadius;
                y--;
                deltaY -= (doubleHorizontalRadius << 1);
                error  -= deltaY;

                if (error <= 0) {
                        x++;
                        deltaX += (doubleVerticalRadius << 1);
                        error  += deltaX;
                }

                plotFourQuadrants(centerX, centerY, x, y);
        }
}

void drawArrow(uint16_t x, uint16_t y, uint16_t angle, uint16_t size_quarter)
{
        float sin_angle = sinf(angle * (float)(M_PI / 180));
        float cos_angle = cosf(angle * (float)(M_PI / 180));
        int16_t peak_x  = (int16_t)(sin_angle * size_quarter * 2);
        int16_t peak_y  = (int16_t)(cos_angle * size_quarter * 2);
        int16_t d_end_x = (int16_t)(cos_angle * size_quarter);
        int16_t d_end_y = (int16_t)(sin_angle * size_quarter);

        write_line_lm(x + peak_x, y - peak_y, x - peak_x - d_end_x, y + peak_y - d_end_y, 1, 1);
        write_line_lm(x + peak_x, y - peak_y, x - peak_x + d_end_x, y + peak_y + d_end_y, 1, 1);
        write_line_lm(x, y, x - peak_x - d_end_x, y + peak_y - d_end_y, 1, 1);
        write_line_lm(x, y, x - peak_x + d_end_x, y + peak_y + d_end_y, 1, 1);
}

void drawBox(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2)
{
        write_line_lm(x1, y1, x2, y1, 1, 1); // top
        write_line_lm(x1, y1, x1, y2, 1, 1); // left
        write_line_lm(x2, y1, x2, y2, 1, 1); // right
        write_line_lm(x1, y2, x2, y2, 1, 1); // bottom
}

#if defined(PIOS_VIDEO_SPLITBUFFER)

void write_pixel(uint8_t *buff, int x, int y, int mode)
{
        CHECK_COORDS(x, y);
        // Determine the bit in the word to be set and the word
        // index to set it in.
        int wordnum = CALC_BUFF_ADDR(x, y);
        uint8_t mask = CALC_BIT_MASK(x);
        WRITE_WORD_MODE(buff, wordnum, mask, mode);
}
#else
void write_pixel(int x, int y, uint8_t value)
{
        CHECK_COORDS(x, y);
        // Determine the bit in the word to be set and the word
        // index to set it in.
        int wordnum = CALC_BUFF_ADDR(x, y);
        uint8_t mask = CALC_BIT_MASK(x);
        WRITE_WORD(draw_buffer, wordnum, mask, value);
}
#endif /* PIOS_VIDEO_SPLITBUFFER */

void write_pixel_lm(int x, int y, int mmode, int lmode)
{
        CHECK_COORDS(x, y);
        // Determine the bit in the word to be set and the word
        // index to set it in.
        int addr   = CALC_BUFF_ADDR(x, y);
        uint8_t mask = CALC_BIT_MASK(x);
#if defined(PIOS_VIDEO_SPLITBUFFER)
        WRITE_WORD_MODE(draw_buffer_mask, addr, mask, mmode);
        WRITE_WORD_MODE(draw_buffer_level, addr, mask, lmode);
#else
        uint8_t value = PACK_BITS(mmode, lmode);
        WRITE_WORD(draw_buffer, addr, mask, value);
#endif /* defined(PIOS_VIDEO_SPLITBUFFER) */
}

#if defined(PIOS_VIDEO_SPLITBUFFER)
void write_hline(uint8_t *buff, int x0, int x1, int y, int mode)
{
        CHECK_COORD_Y(y);
        CLIP_COORD_X(x0);
        CLIP_COORD_X(x1);
        if (x0 > x1) {
                SWAP(x0, x1);
        }
        if (x0 == x1) {
                return;
        }
        /* This is an optimised algorithm for writing horizontal lines.
         * We begin by finding the addresses of the x0 and x1 points. */
        int addr0     = CALC_BUFF_ADDR(x0, y);
        int addr1     = CALC_BUFF_ADDR(x1, y);
        int addr0_bit = CALC_BIT_IN_WORD(x0);
        int addr1_bit = CALC_BIT_IN_WORD(x1);
        int mask, mask_l, mask_r, i;
        /* If the addresses are equal, we only need to write one word
         * which is an island. */
        if (addr0 == addr1) {
                mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
                WRITE_WORD_MODE(buff, addr0, mask, mode);
        } else {
                /* Otherwise we need to write the edges and then the middle. */
                mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit);
                mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit);
                WRITE_WORD_MODE(buff, addr0, mask_l, mode);
                WRITE_WORD_MODE(buff, addr1, mask_r, mode);
                // Now write 0xffff words from start+1 to end-1.
                for (i = addr0 + 1; i <= addr1 - 1; i++) {
                        uint8_t m = 0xff;
                        WRITE_WORD_MODE(buff, i, m, mode);
                }
        }
}
#else
void write_hline(int x0, int x1, int y, uint8_t value)
{
        CHECK_COORD_Y(y);
        CLIP_COORD_X(x0);
        CLIP_COORD_X(x1);
        if (x0 > x1) {
                SWAP(x0, x1);
        }
        if (x0 == x1) {
                return;
        }
        /* This is an optimised algorithm for writing horizontal lines.
         * We begin by finding the addresses of the x0 and x1 points. */
        int addr0     = CALC_BUFF_ADDR(x0, y);
        int addr1     = CALC_BUFF_ADDR(x1, y);
        int addr0_bit = CALC_BIT1_IN_WORD(x0);
        int addr1_bit = CALC_BIT0_IN_WORD(x1);
        int mask, mask_l, mask_r, i;
        /* If the addresses are equal, we only need to write one word
         * which is an island. */
        if (addr0 == addr1) {
                mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
                WRITE_WORD(draw_buffer, addr0, mask, value);
        } else {
                /* Otherwise we need to write the edges and then the middle. */
                mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit);
                mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit);
                WRITE_WORD(draw_buffer, addr0, mask_l, value);
                WRITE_WORD(draw_buffer, addr1, mask_r, value);
                // Now write 0xffff words from start+1 to end-1.
                for (i = addr0 + 1; i <= addr1 - 1; i++) {
                        uint8_t m = 0xff;
                        WRITE_WORD(draw_buffer, i, m, value);
                }
        }
}
#endif /* defined(PIOS_VIDEO_SPLITBUFFER) */


void write_hline_lm(int x0, int x1, int y, int lmode, int mmode)
{
#if defined(PIOS_VIDEO_SPLITBUFFER)
        // TODO: an optimisation would compute the masks and apply to
        // both buffers simultaneously.
        write_hline(draw_buffer_level, x0, x1, y, lmode);
        write_hline(draw_buffer_mask, x0, x1, y, mmode);
#else
        uint8_t value = PACK_BITS(mmode, lmode);
        write_hline(x0, x1, y, value);
#endif /* defined(PIOS_VIDEO_SPLITBUFFER) */
}

void write_hline_outlined(int x0, int x1, int y, int endcap0, int endcap1, int mode, int mmode)
{
        int stroke, fill;

        SETUP_STROKE_FILL(stroke, fill, mode);
        if (x0 > x1) {
                SWAP(x0, x1);
        }
        // Draw the main body of the line.
        write_hline_lm(x0 + 1, x1 - 1, y - 1, stroke, mmode);
        write_hline_lm(x0 + 1, x1 - 1, y + 1, stroke, mmode);
        write_hline_lm(x0 + 1, x1 - 1, y, fill, mmode);
        // Draw the endcaps, if any.
        DRAW_ENDCAP_HLINE(endcap0, x0, y, stroke, fill, mmode);
        DRAW_ENDCAP_HLINE(endcap1, x1, y, stroke, fill, mmode);
}

#if defined(PIOS_VIDEO_SPLITBUFFER)
void write_vline(uint8_t *buff, int x, int y0, int y1, int mode)
{
        CHECK_COORD_X(x);
        CLIP_COORD_Y(y0);
        CLIP_COORD_Y(y1);
        if (y0 > y1) {
                SWAP(y0, y1);
        }
        if (y0 == y1) {
                return;
        }
        /* This is an optimised algorithm for writing vertical lines.
         * We begin by finding the addresses of the x,y0 and x,y1 points. */
        int addr0  = CALC_BUFF_ADDR(x, y0);
        int addr1  = CALC_BUFF_ADDR(x, y1);
        /* Then we calculate the pixel data to be written. */
        uint8_t mask = CALC_BIT_MASK(x);
        /* Run from addr0 to addr1 placing pixels. Increment by the number
         * of words n each graphics line. */
        for (int a = addr0; a <= addr1; a += BUFFER_WIDTH) {
                WRITE_WORD_MODE(buff, a, mask, mode);
        }
}
#else
void write_vline(int x, int y0, int y1, uint8_t value)
{
        CHECK_COORD_X(x);
        CLIP_COORD_Y(y0);
        CLIP_COORD_Y(y1);
        if (y0 > y1) {
                SWAP(y0, y1);
        }
        if (y0 == y1) {
                return;
        }
        /* This is an optimised algorithm for writing vertical lines.
         * We begin by finding the addresses of the x,y0 and x,y1 points. */
        int addr0  = CALC_BUFF_ADDR(x, y0);
        int addr1  = CALC_BUFF_ADDR(x, y1);
        /* Then we calculate the pixel data to be written. */
        uint8_t mask = CALC_BIT_MASK(x);
        /* Run from addr0 to addr1 placing pixels. Increment by the number
         * of words n each graphics line. */
        for (int a = addr0; a <= addr1; a += BUFFER_WIDTH) {
                WRITE_WORD(draw_buffer, a, mask, value);
        }
}
#endif /* defined(PIOS_VIDEO_SPLITBUFFER) */


void write_vline_lm(int x, int y0, int y1, int lmode, int mmode)
{
#if defined(PIOS_VIDEO_SPLITBUFFER)
        // TODO: an optimisation would compute the masks and apply to
        // both buffers simultaneously.
        write_vline(draw_buffer_level, x, y0, y1, lmode);
        write_vline(draw_buffer_mask, x, y0, y1, mmode);
#else
        uint8_t value = PACK_BITS(mmode, lmode);
        write_vline(x, y0, y1, value);
#endif /* defined(PIOS_VIDEO_SPLITBUFFER) */
}

void write_vline_outlined(int x, int y0, int y1, int endcap0, int endcap1, int mode, int mmode)
{
        int stroke, fill;

        if (y0 > y1) {
                SWAP(y0, y1);
        }
        SETUP_STROKE_FILL(stroke, fill, mode);
        // Draw the main body of the line.
        write_vline_lm(x - 1, y0 + 1, y1 - 1, stroke, mmode);
        write_vline_lm(x + 1, y0 + 1, y1 - 1, stroke, mmode);
        write_vline_lm(x, y0 + 1, y1 - 1, fill, mmode);
        // Draw the endcaps, if any.
        DRAW_ENDCAP_VLINE(endcap0, x, y0, stroke, fill, mmode);
        DRAW_ENDCAP_VLINE(endcap1, x, y1, stroke, fill, mmode);
}

#if defined(PIOS_VIDEO_SPLITBUFFER)
void write_filled_rectangle(uint8_t *buff, int x, int y, int width, int height, int mode)
{
        int yy, addr0_old, addr1_old;

        CHECK_COORDS(x, y);
        CHECK_COORDS(x + width, y + height);
        if (width <= 0 || height <= 0) {
                return;
        }
        // Calculate as if the rectangle was only a horizontal line. We then
        // step these addresses through each row until we iterate `height` times.
        int addr0     = CALC_BUFF_ADDR(x, y);
        int addr1     = CALC_BUFF_ADDR(x + width, y);
        int addr0_bit = CALC_BIT_IN_WORD(x);
        int addr1_bit = CALC_BIT_IN_WORD(x + width);
        int mask, mask_l, mask_r, i;
        // If the addresses are equal, we need to write one word vertically.
        if (addr0 == addr1) {
                mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
                while (height--) {
                        WRITE_WORD_MODE(buff, addr0, mask, mode);
                        addr0 += BUFFER_WIDTH;
                }
        } else {
                // Otherwise we need to write the edges and then the middle repeatedly.
                mask_l    = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit);
                mask_r    = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit);
                // Write edges first.
                yy        = 0;
                addr0_old = addr0;
                addr1_old = addr1;
                while (yy < height) {
                        WRITE_WORD_MODE(buff, addr0, mask_l, mode);
                        WRITE_WORD_MODE(buff, addr1, mask_r, mode);
                        addr0 += BUFFER_WIDTH;
                        addr1 += BUFFER_WIDTH;
                        yy++;
                }
                // Now write 0xffff words from start+1 to end-1 for each row.
                yy    = 0;
                addr0 = addr0_old;
                addr1 = addr1_old;
                while (yy < height) {
                        for (i = addr0 + 1; i <= addr1 - 1; i++) {
                                uint8_t m = 0xff;
                                WRITE_WORD_MODE(buff, i, m, mode);
                        }
                        addr0 += BUFFER_WIDTH;
                        addr1 += BUFFER_WIDTH;
                        yy++;
                }
        }
}
#else
void write_filled_rectangle(int x, int y, int width, int height, uint8_t value)
{
        int yy, addr0_old, addr1_old;

        CHECK_COORDS(x, y);
        CHECK_COORDS(x + width, y + height);
        if (width <= 0 || height <= 0) {
                return;
        }
        // Calculate as if the rectangle was only a horizontal line. We then
        // step these addresses through each row until we iterate `height` times.
        int addr0     = CALC_BUFF_ADDR(x, y);
        int addr1     = CALC_BUFF_ADDR(x + width, y);
        int addr0_bit = CALC_BIT_IN_WORD(x);
        int addr1_bit = CALC_BIT_IN_WORD(x + width);
        int mask, mask_l, mask_r, i;
        // If the addresses are equal, we need to write one word vertically.
        if (addr0 == addr1) {
                mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
                while (height--) {
                        WRITE_WORD(draw_buffer, addr0, mask, value);
                        addr0 += BUFFER_WIDTH;
                }
        } else {
                // Otherwise we need to write the edges and then the middle repeatedly.
                mask_l    = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit);
                mask_r    = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit);
                // Write edges first.
                yy        = 0;
                addr0_old = addr0;
                addr1_old = addr1;
                while (yy < height) {
                        WRITE_WORD(draw_buffer, addr0, mask_l, value);
                        WRITE_WORD(draw_buffer, addr1, mask_r, value);
                        addr0 += BUFFER_WIDTH;
                        addr1 += BUFFER_WIDTH;
                        yy++;
                }
                // Now write 0xffff words from start+1 to end-1 for each row.
                yy    = 0;
                addr0 = addr0_old;
                addr1 = addr1_old;
                while (yy < height) {
                        for (i = addr0 + 1; i <= addr1 - 1; i++) {
                                uint8_t m = 0xff;
                                WRITE_WORD(draw_buffer, i, m, value);
                        }
                        addr0 += BUFFER_WIDTH;
                        addr1 += BUFFER_WIDTH;
                        yy++;
                }
        }
}
#endif /* defined(PIOS_VIDEO_SPLITBUFFER) */

void write_filled_rectangle_lm(int x, int y, int width, int height, int lmode, int mmode)
{
#if defined(PIOS_VIDEO_SPLITBUFFER)
        write_filled_rectangle(draw_buffer_mask, x, y, width, height, mmode);
        write_filled_rectangle(draw_buffer_level, x, y, width, height, lmode);
#else
        uint8_t value = PACK_BITS(mmode, lmode);
        write_filled_rectangle(x, y, width, height, value);
#endif /* defined(PIOS_VIDEO_SPLITBUFFER) */
}

void write_rectangle_outlined(int x, int y, int width, int height, int mode, int mmode)
{
        write_hline_outlined(x, x + width, y, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
        write_hline_outlined(x, x + width, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
        write_vline_outlined(x, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
        write_vline_outlined(x + width, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
}

#if defined(PIOS_VIDEO_SPLITBUFFER)

void write_circle(uint8_t *buff, int cx, int cy, int r, int dashp, int mode)
{
        CHECK_COORDS(cx, cy);
        int error = -r, x = r, y = 0;
        while (x >= y) {
                if (dashp == 0 || (y % dashp) < (dashp / 2)) {
                        CIRCLE_PLOT_8(buff, cx, cy, x, y, mode);
                }
                error += (y * 2) + 1;
                y++;
                if (error >= 0) {
                        --x;
                        error -= x * 2;
                }
        }
}

void write_circle_outlined(int cx, int cy, int r, int dashp, int bmode, int mode, int mmode)
{
        int stroke, fill;

        CHECK_COORDS(cx, cy);
        SETUP_STROKE_FILL(stroke, fill, mode);
        // This is a two step procedure. First, we draw the outline of the
        // circle, then we draw the inner part.
        int error = -r, x = r, y = 0;
        while (x >= y) {
                if (dashp == 0 || (y % dashp) < (dashp / 2)) {
                        CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y, mmode);
                        CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y, stroke);
                        CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y + 1, mmode);
                        CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y + 1, stroke);
                        CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y, mmode);
                        CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y, stroke);
                        CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y - 1, mmode);
                        CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y - 1, stroke);
                        if (bmode == 1) {
                                CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y + 1, mmode);
                                CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y + 1, stroke);
                                CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y - 1, mmode);
                                CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y - 1, stroke);
                        }
                }
                error += (y * 2) + 1;
                y++;
                if (error >= 0) {
                        --x;
                        error -= x * 2;
                }
        }
        error = -r;
        x     = r;
        y     = 0;
        while (x >= y) {
                if (dashp == 0 || (y % dashp) < (dashp / 2)) {
                        CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y, mmode);
                        CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y, fill);
                }
                error += (y * 2) + 1;
                y++;
                if (error >= 0) {
                        --x;
                        error -= x * 2;
                }
        }
}

void write_circle_filled(uint8_t *buff, int cx, int cy, int r, int mode)
{
        CHECK_COORDS(cx, cy);
        int error = -r, x = r, y = 0, xch = 0;
        // It turns out that filled circles can take advantage of the midpoint
        // circle algorithm. We simply draw very fast horizontal lines across each
        // pair of X,Y coordinates. In some cases, this can even be faster than
        // drawing an outlined circle!
        //
        // Due to multiple writes to each set of pixels, we have a special exception
        // for when using the toggling draw mode.
        while (x >= y) {
                if (y != 0) {
                        write_hline(buff, cx - x, cx + x, cy + y, mode);
                        write_hline(buff, cx - x, cx + x, cy - y, mode);
                        if (mode != 2 || (mode == 2 && xch && (cx - x) != (cx - y))) {
                                write_hline(buff, cx - y, cx + y, cy + x, mode);
                                write_hline(buff, cx - y, cx + y, cy - x, mode);
                                xch = 0;
                        }
                }
                error += (y * 2) + 1;
                y++;
                if (error >= 0) {
                        --x;
                        xch    = 1;
                        error -= x * 2;
                }
        }
        // Handle toggle mode.
        if (mode == 2) {
                write_hline(buff, cx - r, cx + r, cy, mode);
        }
}
#endif /* defined(PIOS_VIDEO_SPLITBUFFER) */

#if defined(PIOS_VIDEO_SPLITBUFFER)
void write_line(uint8_t *buff, int x0, int y0, int x1, int y1, int mode)
{
        // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
        int steep = abs(y1 - y0) > abs(x1 - x0);

        if (steep) {
                SWAP(x0, y0);
                SWAP(x1, y1);
        }
        if (x0 > x1) {
                SWAP(x0, x1);
                SWAP(y0, y1);
        }
        int deltax     = x1 - x0;
        int deltay = abs(y1 - y0);
        int error      = deltax / 2;
        int ystep;
        int y = y0;
        int x; // , lasty = y, stox = 0;
        if (y0 < y1) {
                ystep = 1;
        } else {
                ystep = -1;
        }
        for (x = x0; x < x1; x++) {
                if (steep) {
                        write_pixel(buff, y, x, mode);
                } else {
                        write_pixel(buff, x, y, mode);
                }
                error -= deltay;
                if (error < 0) {
                        y     += ystep;
                        error += deltax;
                }
        }
}
#else
void write_line(int x0, int y0, int x1, int y1, uint8_t value)
{
        // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
        int steep = abs(y1 - y0) > abs(x1 - x0);

        if (steep) {
                SWAP(x0, y0);
                SWAP(x1, y1);
        }
        if (x0 > x1) {
                SWAP(x0, x1);
                SWAP(y0, y1);
        }
        int deltax     = x1 - x0;
        int deltay = abs(y1 - y0);
        int error      = deltax / 2;
        int ystep;
        int y = y0;
        int x; // , lasty = y, stox = 0;
        if (y0 < y1) {
                ystep = 1;
        } else {
                ystep = -1;
        }
        for (x = x0; x < x1; x++) {
                if (steep) {
                        write_pixel(y, x, value);
                } else {
                        write_pixel(x, y, value);
                }
                error -= deltay;
                if (error < 0) {
                        y     += ystep;
                        error += deltax;
                }
        }
}
#endif /* defined(PIOS_VIDEO_SPLITBUFFER) */

void write_line_lm(int x0, int y0, int x1, int y1, int mmode, int lmode)
{
#if defined(PIOS_VIDEO_SPLITBUFFER)
        write_line(draw_buffer_mask, x0, y0, x1, y1, mmode);
        write_line(draw_buffer_level, x0, y0, x1, y1, lmode);
#else
        uint8_t value = PACK_BITS(mmode, lmode);
        write_line(x0, y0, x1, y1, value);
#endif /* defined(PIOS_VIDEO_SPLITBUFFER) */
}

void write_line_outlined(int x0, int y0, int x1, int y1,
                                                 __attribute__((unused)) int endcap0, __attribute__((unused)) int endcap1,
                                                 int mode, int mmode)
{
        // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
        // This could be improved for speed.
        int omode, imode;

        if (mode == 0) {
                omode = 0;
                imode = 1;
        } else {
                omode = 1;
                imode = 0;
        }
        int steep = abs(y1 - y0) > abs(x1 - x0);
        if (steep) {
                SWAP(x0, y0);
                SWAP(x1, y1);
        }
        if (x0 > x1) {
                SWAP(x0, x1);
                SWAP(y0, y1);
        }
        int deltax     = x1 - x0;
        int deltay = abs(y1 - y0);
        int error      = deltax / 2;
        int ystep;
        int y = y0;
        int x;
        if (y0 < y1) {
                ystep = 1;
        } else {
                ystep = -1;
        }
        // Draw the outline.
        for (x = x0; x < x1; x++) {
                if (steep) {
                        write_pixel_lm(y - 1, x, mmode, omode);
                        write_pixel_lm(y + 1, x, mmode, omode);
                        write_pixel_lm(y, x - 1, mmode, omode);
                        write_pixel_lm(y, x + 1, mmode, omode);
                } else {
                        write_pixel_lm(x - 1, y, mmode, omode);
                        write_pixel_lm(x + 1, y, mmode, omode);
                        write_pixel_lm(x, y - 1, mmode, omode);
                        write_pixel_lm(x, y + 1, mmode, omode);
                }
                error -= deltay;
                if (error < 0) {
                        y     += ystep;
                        error += deltax;
                }
        }
        // Now draw the innards.
        error = deltax / 2;
        y     = y0;
        for (x = x0; x < x1; x++) {
                if (steep) {
                        write_pixel_lm(y, x, mmode, imode);
                } else {
                        write_pixel_lm(x, y, mmode, imode);
                }
                error -= deltay;
                if (error < 0) {
                        y     += ystep;
                        error += deltax;
                }
        }
}

void write_line_outlined_dashed(int x0, int y0, int x1, int y1,
                                                                __attribute__((unused)) int endcap0, __attribute__((unused)) int endcap1,
                                                                int mode, int mmode, int dots)
{
        // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
        // This could be improved for speed.
        int omode, imode;

        if (mode == 0) {
                omode = 0;
                imode = 1;
        } else {
                omode = 1;
                imode = 0;
        }
        int steep = abs(y1 - y0) > abs(x1 - x0);
        if (steep) {
                SWAP(x0, y0);
                SWAP(x1, y1);
        }
        if (x0 > x1) {
                SWAP(x0, x1);
                SWAP(y0, y1);
        }
        int deltax = x1 - x0;
        int deltay = abs(y1 - y0);
        int error  = deltax / 2;
        int ystep;
        int y = y0;
        int x;
        if (y0 < y1) {
                ystep = 1;
        } else {
                ystep = -1;
        }
        // Draw the outline.
        int dot_cnt = 0;
        int draw    = 1;
        for (x = x0; x < x1; x++) {
                if (dots && !(dot_cnt++ % dots)) {
                        draw++;
                }
                if (draw % 2) {
                        if (steep) {
                                write_pixel_lm(y - 1, x, mmode, omode);
                                write_pixel_lm(y + 1, x, mmode, omode);
                                write_pixel_lm(y, x - 1, mmode, omode);
                                write_pixel_lm(y, x + 1, mmode, omode);
                        } else {
                                write_pixel_lm(x - 1, y, mmode, omode);
                                write_pixel_lm(x + 1, y, mmode, omode);
                                write_pixel_lm(x, y - 1, mmode, omode);
                                write_pixel_lm(x, y + 1, mmode, omode);
                        }
                }
                error -= deltay;
                if (error < 0) {
                        y     += ystep;
                        error += deltax;
                }
        }
        // Now draw the innards.
        error = deltax / 2;
        y     = y0;
        dot_cnt = 0;
        draw    = 1;
        for (x = x0; x < x1; x++) {
                if (dots && !(dot_cnt++ % dots)) {
                        draw++;
                }
                if (draw % 2) {
                        if (steep) {
                                write_pixel_lm(y, x, mmode, imode);
                        } else {
                                write_pixel_lm(x, y, mmode, imode);
                        }
                }
                error -= deltay;
                if (error < 0) {
                        y     += ystep;
                        error += deltax;
                }
        }
}

void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff)
{
        uint16_t firstmask = word >> xoff;
        uint16_t lastmask  = word << (16 - xoff);

        WRITE_WORD_NAND(buff, addr + 1, firstmask & 0x00ff);
        WRITE_WORD_NAND(buff, addr, (firstmask & 0xff00) >> 8);
        if (xoff > 0) {
                WRITE_WORD_NAND(buff, addr + 2, (lastmask & 0xff00) >> 8);
        }
}

void write_word_misaligned_OR(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff)
{
        uint16_t firstmask = word >> xoff;
        uint16_t lastmask  = word << (16 - xoff);

        WRITE_WORD_OR(buff, addr + 1, firstmask & 0x00ff);
        WRITE_WORD_OR(buff, addr, (firstmask & 0xff00) >> 8);
        if (xoff > 0) {
                WRITE_WORD_OR(buff, addr + 2, (lastmask & 0xff00) >> 8);
        }
}

void write_word_misaligned_MASKED(uint8_t *buff, uint16_t word, uint16_t mask, unsigned int addr, unsigned int xoff)
{
        uint16_t firstword = (word >> xoff);
        uint16_t lastword  = word << (16 - xoff);
        uint16_t firstmask = (mask >> xoff);
        uint16_t lastmask  = mask << (16 - xoff);

        WRITE_WORD(buff, addr + 1, firstmask & 0x00ff, firstword & 0x00ff);
        WRITE_WORD(buff, addr, (firstmask & 0xff00) >> 8, (firstword & 0xff00) >> 8);
        if (xoff > 0) {
                WRITE_WORD(buff, addr + 2, (lastmask & 0xff00) >> 8, (lastword & 0xff00) >> 8);
        }
}


void write_char(uint8_t ch, int x, int y, const struct FontEntry *font_info)
{
        int yy, row;
#if defined(PIOS_VIDEO_SPLITBUFFER)
        uint16_t levels;
#else
        uint16_t data16;
#endif

        uint16_t mask;
        ch = font_info->lookup[ch];
        if (ch == 255)
                return;

        // check if char is partly out of boundary
        uint8_t partly_out = (x < GRAPHICS_LEFT) || (x + font_info->width > GRAPHICS_RIGHT) || (y < GRAPHICS_TOP) || (y + font_info->height > GRAPHICS_BOTTOM);
        // check if char is totally out of boundary, if so return
        if (partly_out && ((x + font_info->width < GRAPHICS_LEFT) || (x > GRAPHICS_RIGHT) || (y + font_info->height < GRAPHICS_TOP) || (y > GRAPHICS_BOTTOM))) {
                return;
        }

        // Compute starting address of character
        int addr = CALC_BUFF_ADDR(x, y);
        int wbit = CALC_BIT_IN_WORD(x);
        row = ch * font_info->height;

        if (font_info->width > 8) {
                uint32_t data;
                for (yy = y; yy < y + font_info->height; yy++) {
                        if (!partly_out || ((x >= GRAPHICS_LEFT) && (x + font_info->width <= GRAPHICS_RIGHT) && (yy >= GRAPHICS_TOP) && (yy <= GRAPHICS_BOTTOM))) {
                                data = ((uint32_t*)font_info->data)[row];
#if defined(PIOS_VIDEO_SPLITBUFFER)
                                mask = data & 0xFFFF;
                                levels   = (data >> 16) & 0xFFFF;
                                // mask
                                write_word_misaligned_OR(draw_buffer_mask, mask, addr, wbit);
                                // level
                                write_word_misaligned_OR(draw_buffer_level, mask, addr, wbit);
                                mask = (mask & levels);
                                write_word_misaligned_NAND(draw_buffer_level, mask, addr, wbit);
#else
                                data16 = (data & 0xFFFF0000) >> 16;
                                mask = data16 | (data16 << 1);
                                write_word_misaligned_MASKED(draw_buffer, data16, mask, addr, wbit);
                                data16 = (data & 0x0000FFFF);
                                mask = data16 | (data16 << 1);
                                write_word_misaligned_MASKED(draw_buffer, data16, mask, addr + 2, wbit);
#endif /* defined(PIOS_VIDEO_SPLITBUFFER) */
                        }
                        addr += BUFFER_WIDTH;
                        row++;
                }
        }
        else {
                uint16_t data;
                for (yy = y; yy < y + font_info->height; yy++) {
                        if (!partly_out || ((x >= GRAPHICS_LEFT) && (x + font_info->width <= GRAPHICS_RIGHT) && (yy >= GRAPHICS_TOP) && (yy <= GRAPHICS_BOTTOM))) {
                                data = font_info->data[row];
#if defined(PIOS_VIDEO_SPLITBUFFER)
                                levels = data & 0xFF00;
                                mask = (data & 0x00FF) << 8;
                                // mask
                                write_word_misaligned_OR(draw_buffer_mask, mask, addr, wbit);
                                // level
                                write_word_misaligned_OR(draw_buffer_level, mask, addr, wbit);
                                mask = (mask & levels);
                                write_word_misaligned_NAND(draw_buffer_level, mask, addr, wbit);
#else
                                mask = data | (data << 1);
                                write_word_misaligned_MASKED(draw_buffer, data, mask, addr, wbit);
#endif /* defined(PIOS_VIDEO_SPLITBUFFER) */
                        }
                        addr += BUFFER_WIDTH;
                        row++;
                }
        }
}


const struct FontEntry * get_font_info(int font)
{
        if (font >= NUM_FONTS)
                return NULL;
        return fonts[font];
}

void calc_text_dimensions(char *str, const struct FontEntry *font, int xs, int ys, struct FontDimensions *dim)
{
        int max_length = 0, line_length = 0, lines = 1;

        while (*str != 0) {
                line_length++;
                if (*str == '\n' || *str == '\r') {
                        if (line_length > max_length) {
                                max_length = line_length;
                        }
                        line_length = 0;
                        lines++;
                }
                str++;
        }
        if (line_length > max_length) {
                max_length = line_length;
        }
        dim->width  = max_length * (font->width + xs);
        dim->height = lines * (font->height + ys);
}

void write_string(char *str, int x, int y, int xs, int ys, int va, int ha, int flags, int font)
{
        int xx = 0, yy = 0, xx_original = 0;
        const struct FontEntry *font_info;
        struct FontDimensions dim;

        font_info = get_font_info(font);

        calc_text_dimensions(str, font_info, xs, ys, &dim);
        switch (va) {
        case TEXT_VA_TOP:
                yy = y;
                break;
        case TEXT_VA_MIDDLE:
                yy = y - (dim.height / 2) + 1;
                break;
        case TEXT_VA_BOTTOM:
                yy = y - dim.height;
                break;
        }

        switch (ha) {
        case TEXT_HA_LEFT:
                xx = x;
                break;
        case TEXT_HA_CENTER:
                xx = x - (dim.width / 2);
                break;
        case TEXT_HA_RIGHT:
                xx = x - dim.width;
                break;
        }
        // Then write each character.
        xx_original = xx;
        while (*str != 0) {
                if (*str == '\n' || *str == '\r') {
                        yy += ys + font_info->height;
                        xx  = xx_original;
                } else {
                        if (xx >= 0 && xx < GRAPHICS_WIDTH_REAL) {
                                write_char(*str, xx, yy, font_info);
                        }
                        xx += font_info->width + xs;
                }
                str++;
        }
}

void draw_polygon(int16_t x, int16_t y, float angle, const point_t * points, uint8_t n_points, int mode, int mmode)
{
        float sin_angle, cos_angle;
        int16_t x1, y1, x2, y2;

        sin_angle    = sinf(angle * (float)(M_PI / 180));
        cos_angle    = cosf(angle * (float)(M_PI / 180));

        x1 = roundf(cos_angle * points[0].x - sin_angle * points[0].y);
        y1 = roundf(sin_angle * points[0].x + cos_angle * points[0].y);
        x2 = 0; // so compiler doesn't give a warning
        y2 = 0;

        for (int i=0; i<n_points-1; i++)
        {
                x2 = roundf(cos_angle * points[i + 1].x - sin_angle * points[i + 1].y);
                y2 = roundf(sin_angle * points[i + 1].x + cos_angle * points[i + 1].y);

                write_line_outlined(x + x1, y + y1, x + x2, y + y2, 2, 2, mode, mmode);
                x1 = x2;
                y1 = y2;
        }

        x1 = roundf(cos_angle * points[0].x - sin_angle * points[0].y);
        y1 = roundf(sin_angle * points[0].x + cos_angle * points[0].y);
        write_line_outlined(x + x1, y + y1, x + x2, y + y2, 2, 2, mode, mmode);

        for (int i=0; i<n_points-1; i++)
        {
                x2 = roundf(cos_angle * points[i + 1].x - sin_angle * points[i + 1].y);
                y2 = roundf(sin_angle * points[i + 1].x + cos_angle * points[i + 1].y);

                write_line_lm(x + x1, y + y1, x + x2, y + y2, 1, 1);
                x1 = x2;
                y1 = y2;
        }

        x1 = roundf(cos_angle * points[0].x - sin_angle * points[0].y);
        y1 = roundf(sin_angle * points[0].x + cos_angle * points[0].y);

        write_line_lm( x + x1, y + y1, x + x2, y + y2, 1, 1);
}

void lla_to_ned(int32_t lattitude, int32_t longitude, float altitude, float *NED)
{
        // TODO: Abstract out this code and also precompute the part based
        // on home location.

        HomeLocationData homeLocation;
        HomeLocationGet(&homeLocation);

        GPSPositionData gpsPosition;
        GPSPositionGet(&gpsPosition);

        float lat = lattitude / 10.0e6f * DEG2RAD;

        float T[3];
        T[0] = altitude + 6.378137E6f;
        T[1] = cosf(lat) * (altitude + 6.378137E6f);
        T[2] = -1.0f;

        float dL[3] = {(lattitude - homeLocation.Latitude) / 10.0e6f * DEG2RAD,
                                   (longitude - homeLocation.Longitude) / 10.0e6f * DEG2RAD,
                                   (altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude)};

        NED[0] = T[0] * dL[0];
        NED[1] = T[1] * dL[1];
        NED[2] = T[2] * dL[2];
}