OP-1900 added deceleration check to autotakeoff failsafe

[librepilot.git] / flight / modules / Osd / osdgen / osdgen.c

/**
 ******************************************************************************
 * @addtogroup OpenPilotModules OpenPilot Modules
 * @{
 * @addtogroup OSDgenModule osdgen Module
 * @brief Process OSD information
 * @{
 *
 * @file       osdgen.c
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
 * @brief      OSD gen module, handles OSD draw. Parts from CL-OSD and SUPEROSD projects
 * @see        The GNU Public License (GPL) Version 3
 *
 *****************************************************************************/
/*
 * 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, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

// ****************

#include <openpilot.h>

#include "osdgen.h"

#include "attitudestate.h"
#include "gpspositionsensor.h"
#include "homelocation.h"
#include "gpstime.h"
#include "gpssatellites.h"
#include "osdsettings.h"
#include "barosensor.h"
#include "taskinfo.h"
#include "flightstatus.h"

#include "fonts.h"
#include "font12x18.h"
#include "font8x10.h"
#include "WMMInternal.h"

#include "splash.h"
/*
   static uint16_t angleA=0;
   static int16_t angleB=90;
   static int16_t angleC=0;
   static int16_t sum=2;

   static int16_t m_pitch=0;
   static int16_t m_roll=0;
   static int16_t m_yaw=0;
   static int16_t m_batt=0;
   static int16_t m_alt=0;

   static uint8_t m_gpsStatus=0;
   static int32_t m_gpsLat=0;
   static int32_t m_gpsLon=0;
   static float m_gpsAlt=0;
   static float m_gpsSpd=0;*/

extern uint8_t *draw_buffer_level;
extern uint8_t *draw_buffer_mask;
extern uint8_t *disp_buffer_level;
extern uint8_t *disp_buffer_mask;

TTime timex;

// ****************
// Private functions

static void osdgenTask(void *parameters);

// ****************
// Private constants
#define LONG_TIME        0xffff
xSemaphoreHandle osdSemaphore = NULL;

#define STACK_SIZE_BYTES 4096

#define TASK_PRIORITY    (tskIDLE_PRIORITY + 4)
#define UPDATE_PERIOD    100

// ****************
// Private variables

static xTaskHandle osdgenTaskHandle;

struct splashEntry {
    unsigned int   width, height;
    const uint16_t *level;
    const uint16_t *mask;
};

struct splashEntry splash[3] = {
    { oplogo_width,
      oplogo_height,
      oplogo_bits,
      oplogo_mask_bits },
    { level_width,
      level_height,
      level_bits,
      level_mask_bits },
    { llama_width,
      llama_height,
      llama_bits,
      llama_mask_bits }
};

uint16_t mirror(uint16_t source)
{
    int result = ((source & 0x8000) >> 7) | ((source & 0x4000) >> 5) | ((source & 0x2000) >> 3) | ((source & 0x1000) >> 1) | ((source & 0x0800) << 1)
                 | ((source & 0x0400) << 3) | ((source & 0x0200) << 5) | ((source & 0x0100) << 7) | ((source & 0x0080) >> 7) | ((source & 0x0040) >> 5)
                 | ((source & 0x0020) >> 3) | ((source & 0x0010) >> 1) | ((source & 0x0008) << 1) | ((source & 0x0004) << 3) | ((source & 0x0002) << 5)
                 | ((source & 0x0001) << 7);

    return result;
}

void clearGraphics()
{
    memset((uint8_t *)draw_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
    memset((uint8_t *)draw_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
}

void copyimage(uint16_t offsetx, uint16_t offsety, int image)
{
    // check top/left position
    if (!validPos(offsetx, offsety)) {
        return;
    }
    struct splashEntry splash_info;
    splash_info = splash[image];
    offsetx     = offsetx / 8;
    for (uint16_t y = offsety; y < ((splash_info.height) + offsety); y++) {
        uint16_t x1 = offsetx;
        for (uint16_t x = offsetx; x < (((splash_info.width) / 16) + offsetx); x++) {
            draw_buffer_level[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)(
                mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8);
            draw_buffer_level[y * GRAPHICS_WIDTH + x1]     = (uint8_t)(
                mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF);
            draw_buffer_mask[y * GRAPHICS_WIDTH + x1 + 1]  = (uint8_t)(
                mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8);
            draw_buffer_mask[y * GRAPHICS_WIDTH + x1] = (uint8_t)(mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF);
            x1 += 2;
        }
    }
}

uint8_t validPos(uint16_t x, uint16_t y)
{
    if (x < GRAPHICS_HDEADBAND || x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) {
        return 0;
    }
    return 1;
}

// Credit for this one goes to wikipedia! :-)
void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius)
{
    int f     = 1 - radius;
    int ddF_x = 1;
    int ddF_y = -2 * radius;
    int x     = 0;
    int y     = radius;

    write_pixel_lm(x0, y0 + radius, 1, 1);
    write_pixel_lm(x0, y0 - radius, 1, 1);
    write_pixel_lm(x0 + radius, y0, 1, 1);
    write_pixel_lm(x0 - radius, y0, 1, 1);

    while (x < y) {
        // ddF_x == 2 * x + 1;
        // ddF_y == -2 * y;
        // f == x*x + y*y - radius*radius + 2*x - y + 1;
        if (f >= 0) {
            y--;
            ddF_y += 2;
            f     += ddF_y;
        }
        x++;
        ddF_x += 2;
        f     += ddF_x;
        write_pixel_lm(x0 + x, y0 + y, 1, 1);
        write_pixel_lm(x0 - x, y0 + y, 1, 1);
        write_pixel_lm(x0 + x, y0 - y, 1, 1);
        write_pixel_lm(x0 - x, y0 - y, 1, 1);
        write_pixel_lm(x0 + y, y0 + x, 1, 1);
        write_pixel_lm(x0 - y, y0 + x, 1, 1);
        write_pixel_lm(x0 + y, y0 - x, 1, 1);
        write_pixel_lm(x0 - y, y0 - x, 1, 1);
    }
}

void swap(uint16_t *a, uint16_t *b)
{
    uint16_t temp = *a;

    *a = *b;
    *b = temp;
}

static const int8_t sinData[91] =
{ 0,  2,  3,  5,  7,  9,  10,  12,  14,  16,  17,  19, 21, 22, 24, 26, 28, 29, 31, 33, 34, 36, 37, 39, 41, 42, 44, 45, 47, 48, 50, 52, 53, 54, 56, 57, 59, 60, 62, 63, 64,
  66, 67, 68, 69, 71, 72, 73,  74,  75,  77,  78,  79, 80, 81, 82, 83, 84, 85, 86, 87, 87, 88, 89, 90, 91, 91, 92, 93, 93, 94, 95, 95, 96, 96, 97, 97, 97, 98,
  98, 98, 99, 99, 99, 99, 100, 100, 100, 100, 100, 100 };

static int8_t mySin(uint16_t angle)
{
    uint16_t pos = 0;

    pos = angle % 360;
    int8_t mult  = 1;
    // 180-359 is same as 0-179 but negative.
    if (pos >= 180) {
        pos  = pos - 180;
        mult = -1;
    }
    // 0-89 is equal to 90-179 except backwards.
    if (pos >= 90) {
        pos = 180 - pos;
    }
    return mult * (int8_t)(sinData[pos]);
}

static int8_t myCos(uint16_t angle)
{
    return mySin(angle + 90);
}

/// Draws four points relative to the given center point.
///
/// \li centerX + X, centerY + Y
/// \li centerX + X, centerY - Y
/// \li centerX - X, centerY + Y
/// \li centerX - X, centerY - Y
///
/// \param centerX the x coordinate of the center point
/// \param centerY the y coordinate of the center point
/// \param deltaX the difference between the centerX coordinate and each pixel drawn
/// \param deltaY the difference between the centerY coordinate and each pixel drawn
/// \param color the color to draw the pixels with.
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
}

/// Implements the midpoint ellipse drawing algorithm which is a bresenham
/// style DDF.
///
/// \param centerX the x coordinate of the center of the ellipse
/// \param centerY the y coordinate of the center of the ellipse
/// \param horizontalRadius the horizontal radius of the ellipse
/// \param verticalRadius the vertical radius of the ellipse
/// \param color the color of the ellipse border
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)
{
    int16_t a = myCos(angle);
    int16_t b = mySin(angle);

    a = (a * (size / 2)) / 100;
    b = (b * (size / 2)) / 100;
    write_line_lm((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 1, 1); // Direction line
    // write_line_lm((GRAPHICS_SIZE/2)-1 + a/2, (GRAPHICS_SIZE/2)-1 + b/2, (GRAPHICS_SIZE/2)-1 - a/2, (GRAPHICS_SIZE/2)-1 - b/2, 1, 1); //Arrow bottom line
    write_line_lm((x) - 1 + b, (y) - 1 - a, (x) - 1 - a / 2, (y) - 1 - b / 2, 1, 1); // Arrow "wings"
    write_line_lm((x) - 1 + b, (y) - 1 - a, (x) - 1 + a / 2, (y) - 1 + b / 2, 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
}

// simple routines

// SUPEROSD routines, modified

/**
 * write_pixel: Write a pixel at an x,y position to a given surface.
 *
 * @param       buff    pointer to buffer to write in
 * @param       x               x coordinate
 * @param       y               y coordinate
 * @param       mode    0 = clear bit, 1 = set bit, 2 = toggle bit
 */
void write_pixel(uint8_t *buff, unsigned int x, unsigned 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 bitnum    = CALC_BIT_IN_WORD(x);
    int wordnum   = CALC_BUFF_ADDR(x, y);
    // Apply a mask.
    uint16_t mask = 1 << (7 - bitnum);
    WRITE_WORD_MODE(buff, wordnum, mask, mode);
}

/**
 * write_pixel_lm: write the pixel on both surfaces (level and mask.)
 * Uses current draw buffer.
 *
 * @param       x               x coordinate
 * @param       y               y coordinate
 * @param       mmode   0 = clear, 1 = set, 2 = toggle
 * @param       lmode   0 = black, 1 = white, 2 = toggle
 */
void write_pixel_lm(unsigned int x, unsigned 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 bitnum    = CALC_BIT_IN_WORD(x);
    int wordnum   = CALC_BUFF_ADDR(x, y);
    // Apply the masks.
    uint16_t mask = 1 << (7 - bitnum);
    WRITE_WORD_MODE(draw_buffer_mask, wordnum, mask, mmode);
    WRITE_WORD_MODE(draw_buffer_level, wordnum, mask, lmode);
}

/**
 * write_hline: optimised horizontal line writing algorithm
 *
 * @param       buff    pointer to buffer to write in
 * @param       x0              x0 coordinate
 * @param       x1              x1 coordinate
 * @param       y               y coordinate
 * @param       mode    0 = clear, 1 = set, 2 = toggle
 */
void write_hline(uint8_t *buff, unsigned int x0, unsigned int x1, unsigned int y, int mode)
{
    CLIP_COORDS(x0, y);
    CLIP_COORDS(x1, y);
    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);
        }
    }
}

/**
 * write_hline_lm: write both level and mask buffers.
 *
 * @param       x0              x0 coordinate
 * @param       x1              x1 coordinate
 * @param       y               y coordinate
 * @param       lmode   0 = clear, 1 = set, 2 = toggle
 * @param       mmode   0 = clear, 1 = set, 2 = toggle
 */
void write_hline_lm(unsigned int x0, unsigned int x1, unsigned int y, int lmode, int mmode)
{
    // 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);
}

/**
 * write_hline_outlined: outlined horizontal line with varying endcaps
 * Always uses draw buffer.
 *
 * @param       x0                      x0 coordinate
 * @param       x1                      x1 coordinate
 * @param       y                       y coordinate
 * @param       endcap0         0 = none, 1 = single pixel, 2 = full cap
 * @param       endcap1         0 = none, 1 = single pixel, 2 = full cap
 * @param       mode            0 = black outline, white body, 1 = white outline, black body
 * @param       mmode           0 = clear, 1 = set, 2 = toggle
 */
void write_hline_outlined(unsigned int x0, unsigned int x1, unsigned 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);
}

/**
 * write_vline: optimised vertical line writing algorithm
 *
 * @param       buff    pointer to buffer to write in
 * @param       x               x coordinate
 * @param       y0              y0 coordinate
 * @param       y1              y1 coordinate
 * @param       mode    0 = clear, 1 = set, 2 = toggle
 */
void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1, int mode)
{
    unsigned int a;

    CLIP_COORDS(x, y0);
    CLIP_COORDS(x, 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. */
    unsigned int addr0  = CALC_BUFF_ADDR(x, y0);
    unsigned int addr1  = CALC_BUFF_ADDR(x, y1);
    /* Then we calculate the pixel data to be written. */
    unsigned int bitnum = CALC_BIT_IN_WORD(x);
    uint16_t mask = 1 << (7 - bitnum);
    /* Run from addr0 to addr1 placing pixels. Increment by the number
     * of words n each graphics line. */
    for (a = addr0; a <= addr1; a += GRAPHICS_WIDTH_REAL / 8) {
        WRITE_WORD_MODE(buff, a, mask, mode);
    }
}

/**
 * write_vline_lm: write both level and mask buffers.
 *
 * @param       x               x coordinate
 * @param       y0              y0 coordinate
 * @param       y1              y1 coordinate
 * @param       lmode   0 = clear, 1 = set, 2 = toggle
 * @param       mmode   0 = clear, 1 = set, 2 = toggle
 */
void write_vline_lm(unsigned int x, unsigned int y0, unsigned int y1, int lmode, int mmode)
{
    // 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);
}

/**
 * write_vline_outlined: outlined vertical line with varying endcaps
 * Always uses draw buffer.
 *
 * @param       x                       x coordinate
 * @param       y0                      y0 coordinate
 * @param       y1                      y1 coordinate
 * @param       endcap0         0 = none, 1 = single pixel, 2 = full cap
 * @param       endcap1         0 = none, 1 = single pixel, 2 = full cap
 * @param       mode            0 = black outline, white body, 1 = white outline, black body
 * @param       mmode           0 = clear, 1 = set, 2 = toggle
 */
void write_vline_outlined(unsigned int x, unsigned int y0, unsigned 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);
}

/**
 * write_filled_rectangle: draw a filled rectangle.
 *
 * Uses an optimised algorithm which is similar to the horizontal
 * line writing algorithm, but optimised for writing the lines
 * multiple times without recalculating lots of stuff.
 *
 * @param       buff    pointer to buffer to write in
 * @param       x               x coordinate (left)
 * @param       y               y coordinate (top)
 * @param       width   rectangle width
 * @param       height  rectangle height
 * @param       mode    0 = clear, 1 = set, 2 = toggle
 */
void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsigned int width, unsigned int height, int mode)
{
    unsigned int yy, addr0_old, addr1_old;

    CHECK_COORDS(x, y);
    CHECK_COORD_X(x + width);
    CHECK_COORD_Y(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.
    unsigned int addr0     = CALC_BUFF_ADDR(x, y);
    unsigned int addr1     = CALC_BUFF_ADDR(x + width, y);
    unsigned int addr0_bit = CALC_BIT_IN_WORD(x);
    unsigned int addr1_bit = CALC_BIT_IN_WORD(x + width);
    unsigned 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 += GRAPHICS_WIDTH_REAL / 8;
        }
    } 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 += GRAPHICS_WIDTH_REAL / 8;
            addr1 += GRAPHICS_WIDTH_REAL / 8;
            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 += GRAPHICS_WIDTH_REAL / 8;
            addr1 += GRAPHICS_WIDTH_REAL / 8;
            yy++;
        }
    }
}

/**
 * write_filled_rectangle_lm: draw a filled rectangle on both draw buffers.
 *
 * @param       x               x coordinate (left)
 * @param       y               y coordinate (top)
 * @param       width   rectangle width
 * @param       height  rectangle height
 * @param       lmode   0 = clear, 1 = set, 2 = toggle
 * @param       mmode   0 = clear, 1 = set, 2 = toggle
 */
void write_filled_rectangle_lm(unsigned int x, unsigned int y, unsigned int width, unsigned int height, int lmode, int mmode)
{
    write_filled_rectangle(draw_buffer_mask, x, y, width, height, mmode);
    write_filled_rectangle(draw_buffer_level, x, y, width, height, lmode);
}

/**
 * write_rectangle_outlined: draw an outline of a rectangle. Essentially
 * a convenience wrapper for draw_hline_outlined and draw_vline_outlined.
 *
 * @param       x               x coordinate (left)
 * @param       y               y coordinate (top)
 * @param       width   rectangle width
 * @param       height  rectangle height
 * @param       mode    0 = black outline, white body, 1 = white outline, black body
 * @param       mmode   0 = clear, 1 = set, 2 = toggle
 */
void write_rectangle_outlined(unsigned int x, unsigned int y, int width, int height, int mode, int mmode)
{
    // CHECK_COORDS(x, y);
    // CHECK_COORDS(x + width, y + height);
    // if((x + width) > DISP_WIDTH) width = DISP_WIDTH - x;
    // if((y + height) > DISP_HEIGHT) height = DISP_HEIGHT - y;
    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);
}

/**
 * write_circle: draw the outline of a circle on a given buffer,
 * with an optional dash pattern for the line instead of a normal line.
 *
 * @param       buff    pointer to buffer to write in
 * @param       cx              origin x coordinate
 * @param       cy              origin y coordinate
 * @param       r               radius
 * @param       dashp   dash period (pixels) - zero for no dash
 * @param       mode    0 = clear, 1 = set, 2 = toggle
 */
void write_circle(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int r, unsigned 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;
        }
    }
}

/**
 * write_circle_outlined: draw an outlined circle on the draw buffer.
 *
 * @param       cx              origin x coordinate
 * @param       cy              origin y coordinate
 * @param       r               radius
 * @param       dashp   dash period (pixels) - zero for no dash
 * @param       bmode   0 = 4-neighbour border, 1 = 8-neighbour border
 * @param       mode    0 = black outline, white body, 1 = white outline, black body
 * @param       mmode   0 = clear, 1 = set, 2 = toggle
 */
void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, unsigned 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;
        }
    }
}

/**
 * write_circle_filled: fill a circle on a given buffer.
 *
 * @param       buff    pointer to buffer to write in
 * @param       cx              origin x coordinate
 * @param       cy              origin y coordinate
 * @param       r               radius
 * @param       mode    0 = clear, 1 = set, 2 = toggle
 */
void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned 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);
    }
}

/**
 * write_line: Draw a line of arbitrary angle.
 *
 * @param       buff    pointer to buffer to write in
 * @param       x0              first x coordinate
 * @param       y0              first y coordinate
 * @param       x1              second x coordinate
 * @param       y1              second y coordinate
 * @param       mode    0 = clear, 1 = set, 2 = toggle
 */
void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int mode)
{
    // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
    unsigned 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;
    unsigned int deltay = abs(y1 - y0);
    int error      = deltax / 2;
    int ystep;
    unsigned int y = y0;
    unsigned 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;
        }
    }
}

/**
 * write_line_lm: Draw a line of arbitrary angle.
 *
 * @param       x0              first x coordinate
 * @param       y0              first y coordinate
 * @param       x1              second x coordinate
 * @param       y1              second y coordinate
 * @param       mmode   0 = clear, 1 = set, 2 = toggle
 * @param       lmode   0 = clear, 1 = set, 2 = toggle
 */
void write_line_lm(unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int mmode, int lmode)
{
    write_line(draw_buffer_mask, x0, y0, x1, y1, mmode);
    write_line(draw_buffer_level, x0, y0, x1, y1, lmode);
}

/**
 * write_line_outlined: Draw a line of arbitrary angle, with an outline.
 *
 * @param       buff            pointer to buffer to write in
 * @param       x0                      first x coordinate
 * @param       y0                      first y coordinate
 * @param       x1                      second x coordinate
 * @param       y1                      second y coordinate
 * @param       endcap0         0 = none, 1 = single pixel, 2 = full cap
 * @param       endcap1         0 = none, 1 = single pixel, 2 = full cap
 * @param       mode            0 = black outline, white body, 1 = white outline, black body
 * @param       mmode           0 = clear, 1 = set, 2 = toggle
 */
void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsigned 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;
    unsigned int deltay = abs(y1 - y0);
    int error      = deltax / 2;
    int ystep;
    unsigned int y = y0;
    unsigned 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;
        }
    }
}

/**
 * write_word_misaligned: Write a misaligned word across two addresses
 * with an x offset.
 *
 * This allows for many pixels to be set in one write.
 *
 * @param       buff    buffer to write in
 * @param       word    word to write (16 bits)
 * @param       addr    address of first word
 * @param       xoff    x offset (0-15)
 * @param       mode    0 = clear, 1 = set, 2 = toggle
 */
void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff, int mode)
{
    int16_t firstmask = word >> xoff;
    int16_t lastmask  = word << (16 - xoff);

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

/**
 * write_word_misaligned_NAND: Write a misaligned word across two addresses
 * with an x offset, using a NAND mask.
 *
 * This allows for many pixels to be set in one write.
 *
 * @param       buff    buffer to write in
 * @param       word    word to write (16 bits)
 * @param       addr    address of first word
 * @param       xoff    x offset (0-15)
 *
 * This is identical to calling write_word_misaligned with a mode of 0 but
 * it doesn't go through a lot of switch logic which slows down text writing
 * a lot.
 */
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);
    }
}

/**
 * write_word_misaligned_OR: Write a misaligned word across two addresses
 * with an x offset, using an OR mask.
 *
 * This allows for many pixels to be set in one write.
 *
 * @param       buff    buffer to write in
 * @param       word    word to write (16 bits)
 * @param       addr    address of first word
 * @param       xoff    x offset (0-15)
 *
 * This is identical to calling write_word_misaligned with a mode of 1 but
 * it doesn't go through a lot of switch logic which slows down text writing
 * a lot.
 */
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);
    }
}

/**
 * write_word_misaligned_lm: Write a misaligned word across two
 * words, in both level and mask buffers. This is core to the text
 * writing routines.
 *
 * @param       buff    buffer to write in
 * @param       word    word to write (16 bits)
 * @param       addr    address of first word
 * @param       xoff    x offset (0-15)
 * @param       lmode   0 = clear, 1 = set, 2 = toggle
 * @param       mmode   0 = clear, 1 = set, 2 = toggle
 */
void write_word_misaligned_lm(uint16_t wordl, uint16_t wordm, unsigned int addr, unsigned int xoff, int lmode, int mmode)
{
    write_word_misaligned(draw_buffer_level, wordl, addr, xoff, lmode);
    write_word_misaligned(draw_buffer_mask, wordm, addr, xoff, mmode);
}

/**
 * fetch_font_info: Fetch font info structs.
 *
 * @param       ch              character
 * @param       font    font id
 */
int fetch_font_info(uint8_t ch, int font, struct FontEntry *font_info, char *lookup)
{
    // First locate the font struct.
    if ((unsigned int)font > SIZEOF_ARRAY(fonts)) {
        return 0; // font does not exist, exit.
    }
    // Load the font info; IDs are always sequential.
    *font_info = fonts[font];
    // Locate character in font lookup table. (If required.)
    if (lookup != NULL) {
        *lookup = font_info->lookup[ch];
        if (*lookup == 0xff) {
            return 0; // character doesn't exist, don't bother writing it.
        }
    }
    return 1;
}

/**
 * write_char16: Draw a character on the current draw buffer.
 * Currently supports outlined characters and characters with
 * a width of up to 8 pixels.
 *
 * @param       ch              character to write
 * @param       x               x coordinate (left)
 * @param       y               y coordinate (top)
 * @param       flags   flags to write with (see gfx.h)
 * @param       font    font to use
 */
void write_char16(char ch, unsigned int x, unsigned int y, int font)
{
    unsigned int yy, addr_temp, row, row_temp, xshift;
    uint16_t and_mask, or_mask, levels;
    struct FontEntry font_info;

    // char lookup = 0;
    fetch_font_info(0, font, &font_info, NULL);

    // Compute starting address (for x,y) of character.
    int addr = CALC_BUFF_ADDR(x, y);
    int wbit = CALC_BIT_IN_WORD(x);
    // If font only supports lowercase or uppercase, make the letter
    // lowercase or uppercase.
    // How big is the character? We handle characters up to 8 pixels
    // wide for now. Support for large characters may be added in future.
    {
        // Ensure we don't overflow.
        if (x + wbit > GRAPHICS_WIDTH_REAL) {
            return;
        }
        // Load data pointer.
        row       = ch * font_info.height;
        row_temp  = row;
        addr_temp = addr;
        xshift    = 16 - font_info.width;
        // We can write mask words easily.
        for (yy = y; yy < y + font_info.height; yy++) {
            if (font == 3) {
                write_word_misaligned_OR(draw_buffer_mask, font_mask12x18[row] << xshift, addr, wbit);
            } else {
                write_word_misaligned_OR(draw_buffer_mask, font_mask8x10[row] << xshift, addr, wbit);
            }
            addr += GRAPHICS_WIDTH_REAL / 8;
            row++;
        }
        // Level bits are more complicated. We need to set or clear
        // level bits, but only where the mask bit is set; otherwise,
        // we need to leave them alone. To do this, for each word, we
        // construct an AND mask and an OR mask, and apply each individually.
        row  = row_temp;
        addr = addr_temp;
        for (yy = y; yy < y + font_info.height; yy++) {
            if (font == 3) {
                levels   = font_frame12x18[row];
                // if(!(flags & FONT_INVERT)) // data is normally inverted
                levels   = ~levels;
                or_mask  = font_mask12x18[row] << xshift;
                and_mask = (font_mask12x18[row] & levels) << xshift;
            } else {
                levels   = font_frame8x10[row];
                // if(!(flags & FONT_INVERT)) // data is normally inverted
                levels   = ~levels;
                or_mask  = font_mask8x10[row] << xshift;
                and_mask = (font_mask8x10[row] & levels) << xshift;
            }
            write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit);
            // If we're not bold write the AND mask.
            // if(!(flags & FONT_BOLD))
            write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit);
            addr += GRAPHICS_WIDTH_REAL / 8;
            row++;
        }
    }
}

/**
 * write_char: Draw a character on the current draw buffer.
 * Currently supports outlined characters and characters with
 * a width of up to 8 pixels.
 *
 * @param       ch              character to write
 * @param       x               x coordinate (left)
 * @param       y               y coordinate (top)
 * @param       flags   flags to write with (see gfx.h)
 * @param       font    font to use
 */
void write_char(char ch, unsigned int x, unsigned int y, int flags, int font)
{
    unsigned int yy, addr_temp, row, row_temp, xshift;
    uint16_t and_mask, or_mask, levels;
    struct FontEntry font_info;
    char lookup = 0;

    fetch_font_info(ch, font, &font_info, &lookup);
    // Compute starting address (for x,y) of character.
    unsigned int addr = CALC_BUFF_ADDR(x, y);
    unsigned int wbit = CALC_BIT_IN_WORD(x);
    // If font only supports lowercase or uppercase, make the letter
    // lowercase or uppercase.
    /*if(font_info.flags & FONT_LOWERCASE_ONLY)
       ch = tolower(ch);
       if(font_info.flags & FONT_UPPERCASE_ONLY)
       ch = toupper(ch);*/
    fetch_font_info(ch, font, &font_info, &lookup);
    // How big is the character? We handle characters up to 8 pixels
    // wide for now. Support for large characters may be added in future.
    if (font_info.width <= 8) {
        // Ensure we don't overflow.
        if (x + wbit > GRAPHICS_WIDTH_REAL) {
            return;
        }
        // Load data pointer.
        row       = lookup * font_info.height * 2;
        row_temp  = row;
        addr_temp = addr;
        xshift    = 16 - font_info.width;
        // We can write mask words easily.
        for (yy = y; yy < y + font_info.height; yy++) {
            write_word_misaligned_OR(draw_buffer_mask, font_info.data[row] << xshift, addr, wbit);
            addr += GRAPHICS_WIDTH_REAL / 8;
            row++;
        }
        // Level bits are more complicated. We need to set or clear
        // level bits, but only where the mask bit is set; otherwise,
        // we need to leave them alone. To do this, for each word, we
        // construct an AND mask and an OR mask, and apply each individually.
        row  = row_temp;
        addr = addr_temp;
        for (yy = y; yy < y + font_info.height; yy++) {
            levels = font_info.data[row + font_info.height];
            if (!(flags & FONT_INVERT)) {
                // data is normally inverted
                levels = ~levels;
            }
            or_mask  = font_info.data[row] << xshift;
            and_mask = (font_info.data[row] & levels) << xshift;
            write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit);
            // If we're not bold write the AND mask.
            // if(!(flags & FONT_BOLD))
            write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit);
            addr += GRAPHICS_WIDTH_REAL / 8;
            row++;
        }
    }
}

/**
 * calc_text_dimensions: Calculate the dimensions of a
 * string in a given font. Supports new lines and
 * carriage returns in text.
 *
 * @param       str                     string to calculate dimensions of
 * @param       font_info       font info structure
 * @param       xs                      horizontal spacing
 * @param       ys                      vertical spacing
 * @param       dim                     return result: struct FontDimensions
 */
void calc_text_dimensions(char *str, 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);
}

/**
 * write_string: Draw a string on the screen with certain
 * alignment parameters.
 *
 * @param       str             string to write
 * @param       x               x coordinate
 * @param       y               y coordinate
 * @param       xs              horizontal spacing
 * @param       ys              horizontal spacing
 * @param       va              vertical align
 * @param       ha              horizontal align
 * @param       flags   flags (passed to write_char)
 * @param       font    font
 */
void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, int va, int ha, int flags, int font)
{
    int xx = 0, yy = 0, xx_original = 0;
    struct FontEntry font_info;
    struct FontDimensions dim;

    // Determine font info and dimensions/position of the string.
    fetch_font_info(0, font, &font_info, NULL);
    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);
        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) {
                if (font_info.id < 2) {
                    write_char(*str, xx, yy, flags, font);
                } else {
                    write_char16(*str, xx, yy, font);
                }
            }
            xx += font_info.width + xs;
        }
        str++;
    }
}

/**
 * write_string_formatted: Draw a string with format escape
 * sequences in it. Allows for complex text effects.
 *
 * @param       str             string to write (with format data)
 * @param       x               x coordinate
 * @param       y               y coordinate
 * @param       xs              default horizontal spacing
 * @param       ys              default horizontal spacing
 * @param       va              vertical align
 * @param       ha              horizontal align
 * @param       flags   flags (passed to write_char)
 */
void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys,
                            __attribute__((unused)) int va, __attribute__((unused)) int ha, int flags)
{
    int fcode = 0, fptr = 0, font = 0, fwidth = 0, fheight = 0, xx = x, yy = y, max_xx = 0, max_height = 0;
    struct FontEntry font_info;

    // Retrieve sizes of the fonts: bigfont and smallfont.
    fetch_font_info(0, 0, &font_info, NULL);
    int smallfontwidth = font_info.width, smallfontheight = font_info.height;
    fetch_font_info(0, 1, &font_info, NULL);
    int bigfontwidth   = font_info.width, bigfontheight = font_info.height;
    // 11 byte stack with last byte as NUL.
    char fstack[11];
    fstack[10] = '\0';
    // First, we need to parse the string for format characters and
    // work out a bounding box. We'll parse again for the final output.
    // This is a simple state machine parser.
    char *ostr = str;
    while (*str) {
        if (*str == '<' && fcode == 1) {
            // escape code: skip
            fcode = 0;
        }
        if (*str == '<' && fcode == 0) {
            // begin format code?
            fcode = 1;
            fptr  = 0;
        }
        if (*str == '>' && fcode == 1) {
            fcode = 0;
            if (strcmp(fstack, "B")) {
                // switch to "big" font (font #1)
                fwidth  = bigfontwidth;
                fheight = bigfontheight;
            } else if (strcmp(fstack, "S")) {
                // switch to "small" font (font #0)
                fwidth  = smallfontwidth;
                fheight = smallfontheight;
            }
            if (fheight > max_height) {
                max_height = fheight;
            }
            // Skip over this byte. Go to next byte.
            str++;
            continue;
        }
        if (*str != '<' && *str != '>' && fcode == 1) {
            // Add to the format stack (up to 10 bytes.)
            if (fptr > 10) {
                // stop adding bytes
                str++; // go to next byte
                continue;
            }
            fstack[fptr++] = *str;
            fstack[fptr]   = '\0'; // clear next byte (ready for next char or to terminate string.)
        }
        if (fcode == 0) {
            // Not a format code, raw text.
            xx += fwidth + xs;
            if (*str == '\n') {
                if (xx > max_xx) {
                    max_xx = xx;
                }
                xx  = x;
                yy += fheight + ys;
            }
        }
        str++;
    }
    // Reset string pointer.
    str = ostr;
    // Now we've parsed it and got a bbox, we need to work out the dimensions of it
    // and how to align it.
    /*int width = max_xx - x;
       int height = yy - y;
       int ay, ax;
       switch(va)
       {
       case TEXT_VA_TOP:               ay = yy; break;
       case TEXT_VA_MIDDLE:    ay = yy - (height / 2); break;
       case TEXT_VA_BOTTOM:    ay = yy - height; break;
       }
       switch(ha)
       {
       case TEXT_HA_LEFT:              ax = x; break;
       case TEXT_HA_CENTER:    ax = x - (width / 2); break;
       case TEXT_HA_RIGHT:             ax = x - width; break;
       }*/
    // So ax,ay is our new text origin. Parse the text format again and paint
    // the text on the display.
    fcode = 0;
    fptr  = 0;
    font  = 0;
    xx    = 0;
    yy    = 0;
    while (*str) {
        if (*str == '<' && fcode == 1) {
            // escape code: skip
            fcode = 0;
        }
        if (*str == '<' && fcode == 0) {
            // begin format code?
            fcode = 1;
            fptr  = 0;
        }
        if (*str == '>' && fcode == 1) {
            fcode = 0;
            if (strcmp(fstack, "B")) {
                // switch to "big" font (font #1)
                fwidth  = bigfontwidth;
                fheight = bigfontheight;
                font    = 1;
            } else if (strcmp(fstack, "S")) {
                // switch to "small" font (font #0)
                fwidth  = smallfontwidth;
                fheight = smallfontheight;
                font    = 0;
            }
            // Skip over this byte. Go to next byte.
            str++;
            continue;
        }
        if (*str != '<' && *str != '>' && fcode == 1) {
            // Add to the format stack (up to 10 bytes.)
            if (fptr > 10) {
                // stop adding bytes
                str++; // go to next byte
                continue;
            }
            fstack[fptr++] = *str;
            fstack[fptr]   = '\0'; // clear next byte (ready for next char or to terminate string.)
        }
        if (fcode == 0) {
            // Not a format code, raw text. So we draw it.
            // TODO - different font sizes.
            write_char(*str, xx, yy + (max_height - fheight), flags, font);
            xx += fwidth + xs;
            if (*str == '\n') {
                if (xx > max_xx) {
                    max_xx = xx;
                }
                xx  = x;
                yy += fheight + ys;
            }
        }
        str++;
    }
}

// SUPEROSD-

// graphics

void drawAttitude(uint16_t x, uint16_t y, int16_t pitch, int16_t roll, uint16_t size)
{
    int16_t a = mySin(roll + 360);
    int16_t b = myCos(roll + 360);
    int16_t c = mySin(roll + 90 + 360) * 5 / 100;
    int16_t d = myCos(roll + 90 + 360) * 5 / 100;

    int16_t k;
    int16_t l;

    int16_t indi30x1 = myCos(30) * (size / 2 + 1) / 100;
    int16_t indi30y1 = mySin(30) * (size / 2 + 1) / 100;

    int16_t indi30x2 = myCos(30) * (size / 2 + 4) / 100;
    int16_t indi30y2 = mySin(30) * (size / 2 + 4) / 100;

    int16_t indi60x1 = myCos(60) * (size / 2 + 1) / 100;
    int16_t indi60y1 = mySin(60) * (size / 2 + 1) / 100;

    int16_t indi60x2 = myCos(60) * (size / 2 + 4) / 100;
    int16_t indi60y2 = mySin(60) * (size / 2 + 4) / 100;

    pitch = pitch % 90;
    if (pitch > 90) {
        pitch = pitch - 90;
    }
    if (pitch < -90) {
        pitch = pitch + 90;
    }
    a = (a * (size / 2)) / 100;
    b = (b * (size / 2)) / 100;

    if (roll < -90 || roll > 90) {
        pitch = pitch * -1;
    }
    k = a * pitch / 90;
    l = b * pitch / 90;

    // scale
    // 0
    // drawLine((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1);
    // drawLine((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1);
    write_line_outlined((x) - 1 - (size / 2 + 4), (y) - 1, (x) - 1 - (size / 2 + 1), (y) - 1, 0, 0, 0, 1);
    write_line_outlined((x) - 1 + (size / 2 + 4), (y) - 1, (x) - 1 + (size / 2 + 1), (y) - 1, 0, 0, 0, 1);

    // 30
    // drawLine((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2);
    // drawLine((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2);
    write_line_outlined((x) - 1 + indi30x1, (y) - 1 - indi30y1, (x) - 1 + indi30x2, (y) - 1 - indi30y2, 0, 0, 0, 1);
    write_line_outlined((x) - 1 - indi30x1, (y) - 1 - indi30y1, (x) - 1 - indi30x2, (y) - 1 - indi30y2, 0, 0, 0, 1);
    // 60
    // drawLine((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2);
    // drawLine((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2);
    write_line_outlined((x) - 1 + indi60x1, (y) - 1 - indi60y1, (x) - 1 + indi60x2, (y) - 1 - indi60y2, 0, 0, 0, 1);
    write_line_outlined((x) - 1 - indi60x1, (y) - 1 - indi60y1, (x) - 1 - indi60x2, (y) - 1 - indi60y2, 0, 0, 0, 1);
    // 90
    // drawLine((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1));
    write_line_outlined((x) - 1, (y) - 1 - (size / 2 + 4), (x) - 1, (y) - 1 - (size / 2 + 1), 0, 0, 0, 1);

    // roll
    // drawLine((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a); //Direction line
    write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1); // Direction line
    // "wingtips"
    // drawLine((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c);
    // drawLine((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a);
    write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 - b + d, (y) - 1 + a - c, 0, 0, 0, 1);
    write_line_outlined((x) - 1 + b + d, (y) - 1 - a - c, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1);

    // pitch
    // drawLine((x)-1, (y)-1, (x)-1 - k, (y)-1 - l);
    write_line_outlined((x) - 1, (y) - 1, (x) - 1 - k, (y) - 1 - l, 0, 0, 0, 1);

    // drawCircle(x-1, y-1, 5);
    // write_circle_outlined(x-1, y-1, 5,0,0,0,1);
    // drawCircle(x-1, y-1, size/2+4);
    // write_circle_outlined(x-1, y-1, size/2+4,0,0,0,1);
}

void drawBattery(uint16_t x, uint16_t y, uint8_t battery, uint16_t size)
{
    int i = 0;
    int batteryLines;

    // top
    /*drawLine((x)-1+(size/2-size/4), (y)-1, (x)-1 + (size/2+size/4), (y)-1);
       drawLine((x)-1+(size/2-size/4), (y)-1+1, (x)-1 + (size/2+size/4), (y)-1+1);

       drawLine((x)-1, (y)-1+2, (x)-1 + size, (y)-1+2);
       //bottom
       drawLine((x)-1, (y)-1+size*3, (x)-1 + size, (y)-1+size*3);
       //left
       drawLine((x)-1, (y)-1+2, (x)-1, (y)-1+size*3);

       //right
       drawLine((x)-1+size, (y)-1+2, (x)-1+size, (y)-1+size*3);*/

    write_rectangle_outlined((x) - 1, (y) - 1 + 2, size, size * 3, 0, 1);
    write_vline_lm((x) - 1 + (size / 2 + size / 4) + 1, (y) - 2, (y) - 1 + 1, 0, 1);
    write_vline_lm((x) - 1 + (size / 2 - size / 4) - 1, (y) - 2, (y) - 1 + 1, 0, 1);
    write_hline_lm((x) - 1 + (size / 2 - size / 4), (x) - 1 + (size / 2 + size / 4), (y) - 2, 0, 1);
    write_hline_lm((x) - 1 + (size / 2 - size / 4), (x) - 1 + (size / 2 + size / 4), (y) - 1, 1, 1);
    write_hline_lm((x) - 1 + (size / 2 - size / 4), (x) - 1 + (size / 2 + size / 4), (y) - 1 + 1, 1, 1);

    batteryLines = battery * (size * 3 - 2) / 100;
    for (i = 0; i < batteryLines; i++) {
        write_hline_lm((x) - 1, (x) - 1 + size, (y) - 1 + size * 3 - i, 1, 1);
    }
}

void printTime(uint16_t x, uint16_t y)
{
    char temp[9] =
    { 0 };

    sprintf(temp, "%02d:%02d:%02d", timex.hour, timex.min, timex.sec);
    // printTextFB(x,y,temp);
    write_string(temp, x, y, 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
}

/*
   void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) {

   char temp[9]={0};
   char updown=' ';
   uint16_t charx=x/16;
   if(dir==0)
   updown=24;
   if(dir==1)
   updown=25;
   sprintf(temp,"%c%6dm",updown,alt);
   printTextFB(charx,y+2,temp);
   // frame
   drawBox(charx*16-3,y,charx*16+strlen(temp)*8+3,y+11);
   }*/

/**
 * hud_draw_vertical_scale: Draw a vertical scale.
 *
 * @param       v                               value to display as an integer
 * @param       range                   range about value to display (+/- range/2 each direction)
 * @param       halign                  horizontal alignment: -1 = left, +1 = right.
 * @param       x                       x displacement (typ. 0)
 * @param       y                       y displacement (typ. half display height)
 * @param       height                  height of scale
 * @param       mintick_step                    how often a minor tick is shown
 * @param       majtick_step                    how often a major tick is shown
 * @param       mintick_len             minor tick length
 * @param       majtick_len             major tick length
 * @param       boundtick_len           boundary tick length
 * @param       max_val                 maximum expected value (used to compute size of arrow ticker)
 * @param       flags                   special flags (see hud.h.)
 */
void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int height, int mintick_step, int majtick_step, int mintick_len, int majtick_len,
                             int boundtick_len, __attribute__((unused)) int max_val, int flags)
{
    char temp[15]; // , temp2[15];
    struct FontEntry font_info;
    struct FontDimensions dim;
    // Halign should be in a small span.
    // MY_ASSERT(halign >= -1 && halign <= 1);
    // Compute the position of the elements.
    int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, boundtick_start = 0, boundtick_end = 0;

    if (halign == -1) {
        majtick_start   = x;
        majtick_end     = x + majtick_len;
        mintick_start   = x;
        mintick_end     = x + mintick_len;
        boundtick_start = x;
        boundtick_end   = x + boundtick_len;
    } else if (halign == +1) {
        x = x - GRAPHICS_HDEADBAND;
        majtick_start   = GRAPHICS_WIDTH_REAL - x - 1;
        majtick_end     = GRAPHICS_WIDTH_REAL - x - majtick_len - 1;
        mintick_start   = GRAPHICS_WIDTH_REAL - x - 1;
        mintick_end     = GRAPHICS_WIDTH_REAL - x - mintick_len - 1;
        boundtick_start = GRAPHICS_WIDTH_REAL - x - 1;
        boundtick_end   = GRAPHICS_WIDTH_REAL - x - boundtick_len - 1;
    }
    // Retrieve width of large font (font #0); from this calculate the x spacing.
    fetch_font_info(0, 0, &font_info, NULL);
    int arrow_len      = (font_info.height / 2) + 1;             // FIXME, font info being loaded correctly??
    int text_x_spacing = arrow_len;
    int max_text_y     = 0, text_length = 0;
    int small_font_char_width = font_info.width + 1; // +1 for horizontal spacing = 1
    // For -(range / 2) to +(range / 2), draw the scale.
    int range_2 = range / 2; // , height_2 = height / 2;
    int r = 0, rr = 0, rv = 0, ys = 0, style = 0; // calc_ys = 0,
    // Iterate through each step.
    for (r = -range_2; r <= +range_2; r++) {
        style = 0;
        rr    = r + range_2 - v; // normalise range for modulo, subtract value to move ticker tape
        rv    = -rr + range_2; // for number display
        if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE) {
            rr += majtick_step / 2;
        }
        if (rr % majtick_step == 0) {
            style = 1; // major tick
        } else if (rr % mintick_step == 0) {
            style = 2; // minor tick
        } else {
            style = 0;
        }
        if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0) {
            continue;
        }
        if (style) {
            // Calculate y position.
            ys = ((long int)(r * height) / (long int)range) + y;
            // sprintf(temp, "ys=%d", ys);
            // con_puts(temp, 0);
            // Depending on style, draw a minor or a major tick.
            if (style == 1) {
                write_hline_outlined(majtick_start, majtick_end, ys, 2, 2, 0, 1);
                memset(temp, ' ', 10);
                // my_itoa(rv, temp);
                sprintf(temp, "%d", rv);
                text_length = (strlen(temp) + 1) * small_font_char_width; // add 1 for margin
                if (text_length > max_text_y) {
                    max_text_y = text_length;
                }
                if (halign == -1) {
                    write_string(temp, majtick_end + text_x_spacing, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 1);
                } else {
                    write_string(temp, majtick_end - text_x_spacing + 1, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 1);
                }
            } else if (style == 2) {
                write_hline_outlined(mintick_start, mintick_end, ys, 2, 2, 0, 1);
            }
        }
    }
    // Generate the string for the value, as well as calculating its dimensions.
    memset(temp, ' ', 10);
    // my_itoa(v, temp);
    sprintf(temp, "%d", v);
    // TODO: add auto-sizing.
    calc_text_dimensions(temp, font_info, 1, 0, &dim);
    int xx = 0, i = 0;
    if (halign == -1) {
        xx = majtick_end + text_x_spacing;
    } else {
        xx = majtick_end - text_x_spacing;
    }
    // Draw an arrow from the number to the point.
    for (i = 0; i < arrow_len; i++) {
        if (halign == -1) {
            write_pixel_lm(xx - arrow_len + i, y - i - 1, 1, 1);
            write_pixel_lm(xx - arrow_len + i, y + i - 1, 1, 1);
            write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y - i - 1, 0, 1);
            write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y + i - 1, 0, 1);
        } else {
            write_pixel_lm(xx + arrow_len - i, y - i - 1, 1, 1);
            write_pixel_lm(xx + arrow_len - i, y + i - 1, 1, 1);
            write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y - i - 1, 0, 1);
            write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y + i - 1, 0, 1);
        }
        // FIXME
        // write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y - i - 1, 1, 1);
        // write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y + i - 1, 1, 1);
    }
    if (halign == -1) {
        write_hline_lm(xx, xx + dim.width - 1, y - arrow_len, 1, 1);
        write_hline_lm(xx, xx + dim.width - 1, y + arrow_len - 2, 1, 1);
        write_vline_lm(xx + dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1);
    } else {
        write_hline_lm(xx, xx - dim.width - 1, y - arrow_len, 1, 1);
        write_hline_lm(xx, xx - dim.width - 1, y + arrow_len - 2, 1, 1);
        write_vline_lm(xx - dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1);
    }
    // Draw the text.
    if (halign == -1) {
        write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 0);
    } else {
        write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 0);
    }
    // Then, add a slow cut off on the edges, so the text doesn't sharply
    // disappear. We simply clear the areas above and below the ticker, and we
    // use little markers on the edges.
    if (halign == -1) {
        write_filled_rectangle_lm(majtick_end + text_x_spacing, y + (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0,
                                  0);
        write_filled_rectangle_lm(majtick_end + text_x_spacing, y - (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0,
                                  0);
    } else {
        write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y + (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0);
        write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y - (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0);
    }
    write_hline_outlined(boundtick_start, boundtick_end, y + (height / 2), 2, 2, 0, 1);
    write_hline_outlined(boundtick_start, boundtick_end, y - (height / 2), 2, 2, 0, 1);
}

/**
 * hud_draw_compass: Draw a compass.
 *
 * @param       v                               value for the compass
 * @param       range                   range about value to display (+/- range/2 each direction)
 * @param       width                   length in pixels
 * @param       x                               x displacement (typ. half display width)
 * @param       y                               y displacement (typ. bottom of display)
 * @param       mintick_step    how often a minor tick is shown
 * @param       majtick_step    how often a major tick (heading "xx") is shown
 * @param       mintick_len             minor tick length
 * @param       majtick_len             major tick length
 * @param       flags                   special flags (see hud.h.)
 */
void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mintick_step, int majtick_step, int mintick_len, int majtick_len, __attribute__((unused)) int flags)
{
    v %= 360; // wrap, just in case.
    struct FontEntry font_info;
    int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, textoffset = 0;
    char headingstr[4];
    majtick_start = y;
    majtick_end   = y - majtick_len;
    mintick_start = y;
    mintick_end   = y - mintick_len;
    textoffset    = 8;
    int r, style, rr, xs; // rv,
    int range_2 = range / 2;
    for (r = -range_2; r <= +range_2; r++) {
        style = 0;
        rr    = (v + r + 360) % 360; // normalise range for modulo, add to move compass track
        // rv = -rr + range_2; // for number display
        if (rr % majtick_step == 0) {
            style = 1; // major tick
        } else if (rr % mintick_step == 0) {
            style = 2; // minor tick
        }
        if (style) {
            // Calculate x position.
            xs = ((long int)(r * width) / (long int)range) + x;
            // Draw it.
            if (style == 1) {
                write_vline_outlined(xs, majtick_start, majtick_end, 2, 2, 0, 1);
                // Draw heading above this tick.
                // If it's not one of north, south, east, west, draw the heading.
                // Otherwise, draw one of the identifiers.
                if (rr % 90 != 0) {
                    // We abbreviate heading to two digits. This has the side effect of being easy to compute.
                    headingstr[0] = '0' + (rr / 100);
                    headingstr[1] = '0' + ((rr / 10) % 10);
                    headingstr[2] = 0;
                    headingstr[3] = 0; // nul to terminate
                } else {
                    switch (rr) {
                    case 0:
                        headingstr[0] = 'N';
                        break;
                    case 90:
                        headingstr[0] = 'E';
                        break;
                    case 180:
                        headingstr[0] = 'S';
                        break;
                    case 270:
                        headingstr[0] = 'W';
                        break;
                    }
                    headingstr[1] = 0;
                    headingstr[2] = 0;
                    headingstr[3] = 0;
                }
                // +1 fudge...!
                write_string(headingstr, xs + 1, majtick_start + textoffset, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 0, 1);
            } else if (style == 2) {
                write_vline_outlined(xs, mintick_start, mintick_end, 2, 2, 0, 1);
            }
        }
    }
    // Then, draw a rectangle with the present heading in it.
    // We want to cover up any other markers on the bottom.
    // First compute font size.
    fetch_font_info(0, 3, &font_info, NULL);
    int text_width = (font_info.width + 1) * 3;
    int rect_width = text_width + 2;
    write_filled_rectangle_lm(x - (rect_width / 2), majtick_start + 2, rect_width, font_info.height + 2, 0, 1);
    write_rectangle_outlined(x - (rect_width / 2), majtick_start + 2, rect_width, font_info.height + 2, 0, 1);
    headingstr[0] = '0' + (v / 100);
    headingstr[1] = '0' + ((v / 10) % 10);
    headingstr[2] = '0' + (v % 10);
    headingstr[3] = 0;
    write_string(headingstr, x + 1, majtick_start + textoffset + 2, 0, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 1, 3);
}
// CORE draw routines end here

void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, int16_t size)
{
    float alpha;
    uint8_t vertical = 0, horizontal = 0;
    int16_t x1, x2;
    int16_t y1, y2;
    int16_t refx, refy;

    alpha = DEG2RAD(angle);
    refx  = l_x + size / 2;
    refy  = l_y + size / 2;

    //
    float k    = 0;
    float dx   = sinf(alpha) * (pitch / 90.0f * (size / 2));
    float dy   = cosf(alpha) * (pitch / 90.0f * (size / 2));
    int16_t x0 = (size / 2) - dx;
    int16_t y0 = (size / 2) + dy;
    // calculate the line function
    if ((angle < 90.0f) && (angle > -90)) {
        vertical = 0;
        if (fabsf(angle) < 1e-5f) {
            horizontal = 1;
        } else {
            k = tanf(alpha);
        }
    } else {
        vertical = 1;
    }

    // crossing point of line
    if (!vertical && !horizontal) {
        // y-y0=k(x-x0)
        int16_t x = 0;
        int16_t y = k * (x - x0) + y0;
        // find right crossing point
        x1 = x;
        y1 = y;
        if (y < 0) {
            y1 = 0;
            x1 = ((y1 - y0) + k * x0) / k;
        }
        if (y > size) {
            y1 = size;
            x1 = ((y1 - y0) + k * x0) / k;
        }
        // left crossing point
        x  = size;
        y  = k * (x - x0) + y0;
        x2 = x;
        y2 = y;
        if (y < 0) {
            y2 = 0;
            x2 = ((y2 - y0) + k * x0) / k;
        }
        if (y > size) {
            y2 = size;
            x2 = ((y2 - y0) + k * x0) / k;
        }
        // move to location
        // horizon line
        write_line_outlined(x1 + l_x, y1 + l_y, x2 + l_x, y2 + l_y, 0, 0, 0, 1);
        // fill
        if (angle <= 0.0f && angle > -90.0f) {
            // write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
            for (int i = y2; i < size; i++) {
                x2 = ((i - y0) + k * x0) / k;
                if (x2 > size) {
                    x2 = size;
                }
                if (x2 < 0) {
                    x2 = 0;
                }
                write_hline_lm(x2 + l_x, size + l_x, i + l_y, 1, 1);
            }
        } else if (angle < -90.0f) {
            // write_string("2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
            for (int i = 0; i < y2; i++) {
                x2 = ((i - y0) + k * x0) / k;
                if (x2 > size) {
                    x2 = size;
                }
                if (x2 < 0) {
                    x2 = 0;
                }
                write_hline_lm(size + l_x, x2 + l_x, i + l_y, 1, 1);
            }
        } else if (angle > 0.0f && angle < 90.0f) {
            // write_string("3", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
            for (int i = y1; i < size; i++) {
                x2 = ((i - y0) + k * x0) / k;
                if (x2 > size) {
                    x2 = size;
                }
                if (x2 < 0) {
                    x2 = 0;
                }
                write_hline_lm(0 + l_x, x2 + l_x, i + l_y, 1, 1);
            }
        } else if (angle > 90.0f) {
            // write_string("4", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
            for (int i = 0; i < y1; i++) {
                x2 = ((i - y0) + k * x0) / k;
                if (x2 > size) {
                    x2 = size;
                }
                if (x2 < 0) {
                    x2 = 0;
                }
                write_hline_lm(x2 + l_x, 0 + l_x, i + l_y, 1, 1);
            }
        }
    } else if (vertical) {
        // horizon line
        write_line_outlined(x0 + l_x, 0 + l_y, x0 + l_x, size + l_y, 0, 0, 0, 1);
        if (angle >= 90.0f) {
            // write_string("5", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
            for (int i = 0; i < size; i++) {
                write_hline_lm(0 + l_x, x0 + l_x, i + l_y, 1, 1);
            }
        } else {
            // write_string("6", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
            for (int i = 0; i < size; i++) {
                write_hline_lm(size + l_x, x0 + l_x, i + l_y, 1, 1);
            }
        }
    } else if (horizontal) {
        // horizon line
        write_hline_outlined(0 + l_x, size + l_x, y0 + l_y, 0, 0, 0, 1);
        if (angle < 0) {
            // write_string("7", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
            for (int i = 0; i < y0; i++) {
                write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1);
            }
        } else {
            // write_string("8", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
            for (int i = y0; i < size; i++) {
                write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1);
            }
        }
    }

    // sides
    write_line_outlined(l_x, l_y, l_x, l_y + size, 0, 0, 0, 1);
    write_line_outlined(l_x + size, l_y, l_x + size, l_y + size, 0, 0, 0, 1);
    // plane
    write_line_outlined(refx - 5, refy, refx + 6, refy, 0, 0, 0, 1);
    write_line_outlined(refx, refy, refx, refy - 3, 0, 0, 0, 1);
}

void introText()
{
    write_string("ver 0.2", APPLY_HDEADBAND((GRAPHICS_RIGHT / 2)), APPLY_VDEADBAND(GRAPHICS_BOTTOM - 10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
}

void introGraphics()
{
    /* logo */
    int image = 0;
    struct splashEntry splash_info;

    splash_info = splash[image];

    copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - (splash_info.width) / 2), APPLY_VDEADBAND(GRAPHICS_BOTTOM / 2 - (splash_info.height) / 2), image);

    /* frame */
    drawBox(APPLY_HDEADBAND(0), APPLY_VDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT - 8), APPLY_VDEADBAND(GRAPHICS_BOTTOM));

    // Must mask out last half-word because SPI keeps clocking it out otherwise
    for (uint32_t i = 0; i < 8; i++) {
        write_vline(draw_buffer_level, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
        write_vline(draw_buffer_mask, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
    }
}

void calcHomeArrow(int16_t m_yaw)
{
    HomeLocationData home;

    HomeLocationGet(&home);
    GPSPositionSensorData gpsData;
    GPSPositionSensorGet(&gpsData);

    /** http://www.movable-type.co.uk/scripts/latlong.html **/
    float lat1, lat2, lon1, lon2, a, c, d, x, y, brng, u2g;
    float elevation;
    float gcsAlt = home.Altitude; // Home MSL altitude
    float uavAlt = gpsData.Altitude; // UAV MSL altitude
    float dAlt   = uavAlt - gcsAlt; // Altitude difference

    // Convert to radians
    lat1 = DEG2RAD(home.Latitude) / 10000000.0f; // Home lat
    lon1 = DEG2RAD(home.Longitude) / 10000000.0f; // Home lon
    lat2 = DEG2RAD(gpsData.Latitude) / 10000000.0f; // UAV lat
    lon2 = DEG2RAD(gpsData.Longitude) / 10000000.0f; // UAV lon

    // Bearing
    /**
       var y = Math.sin(dLon) * Math.cos(lat2);
       var x = Math.cos(lat1)*Math.sin(lat2) -
       Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
       var brng = Math.atan2(y, x).toDeg();
     **/
    y    = sinf(lon2 - lon1) * cosf(lat2);
    x    = cosf(lat1) * sinf(lat2) - sinf(lat1) * cosf(lat2) * cosf(lon2 - lon1);
    brng = RAD2DEG(atan2f(y, x));
    if (brng < 0) {
        brng += 360.0f;
    }

    // yaw corrected bearing, needs compass
    u2g = brng - 180.0f - m_yaw;
    if (u2g < 0) {
        u2g += 360.0f;
    }

    // Haversine formula for distance
    /**
       var R = 6371; // km
       var dLat = (lat2-lat1).toRad();
       var dLon = (lon2-lon1).toRad();
       var a = Math.sin(dLat/2) * Math.sin(dLat/2) +
       Math.cos(lat1.toRad()) * Math.cos(lat2.toRad()) *
       Math.sin(dLon/2) * Math.sin(dLon/2);
       var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
       var d = R * c;
     **/
    a = sinf((lat2 - lat1) / 2) * sinf((lat2 - lat1) / 2) + cosf(lat1) * cosf(lat2) * sinf((lon2 - lon1) / 2) * sinf((lon2 - lon1) / 2);
    c = 2.0f * atan2f(sqrtf(a), sqrtf(1.0f - a));
    d = 6371.0f * 1000.0f * c;

    // Elevation  v depends servo direction
    if (d > 0.0f) {
        elevation = 90.0f - RAD2DEG(atanf(dAlt / d));
    } else {
        elevation = 0.0f;
    }
    // ! TODO: sanity check

    char temp[50] =
    { 0 };
    sprintf(temp, "hea:%d", (int)brng);
    write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - 30), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
    sprintf(temp, "ele:%d", (int)elevation);
    write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - 30), APPLY_VDEADBAND(30 + 10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
    sprintf(temp, "dis:%d", (int)d);
    write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - 30), APPLY_VDEADBAND(30 + 10 + 10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
    sprintf(temp, "u2g:%d", (int)u2g);
    write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - 30), APPLY_VDEADBAND(30 + 10 + 10 + 10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);

    sprintf(temp, "%c%c", (int)(u2g / 22.5f) * 2 + 0x90, (int)(u2g / 22.5f) * 2 + 0x91);
    write_string(temp, APPLY_HDEADBAND(250), APPLY_VDEADBAND(40 + 10 + 10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
}

int lama = 10;
int lama_loc[2][30];

void lamas(void)
{
    char temp[10] =
    { 0 };

    lama++;
    if (lama % 10 == 0) {
        for (int z = 0; z < 30; z++) {
            lama_loc[0][z] = rand() % (GRAPHICS_RIGHT - 10);
            lama_loc[1][z] = rand() % (GRAPHICS_BOTTOM - 10);
        }
    }
    for (int z = 0; z < 30; z++) {
        sprintf(temp, "%c", 0xe8 + (lama_loc[0][z] % 2));
        write_string(temp, APPLY_HDEADBAND(lama_loc[0][z]), APPLY_VDEADBAND(lama_loc[1][z]), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
    }
}

// main draw function
void updateGraphics()
{
    OsdSettingsData OsdSettings;

    OsdSettingsGet(&OsdSettings);
    AttitudeStateData attitude;
    AttitudeStateGet(&attitude);
    GPSPositionSensorData gpsData;
    GPSPositionSensorGet(&gpsData);
    HomeLocationData home;
    HomeLocationGet(&home);
    BaroSensorData baro;
    BaroSensorGet(&baro);
    FlightStatusData status;
    FlightStatusGet(&status);

    PIOS_Servo_Set(0, OsdSettings.White);
    PIOS_Servo_Set(1, OsdSettings.Black);

    switch (OsdSettings.Screen) {
    case 0: // Dave simple
    {
        if (home.Set == HOMELOCATION_SET_FALSE) {
            char temps[20] =
            { 0 };
            sprintf(temps, "HOME NOT SET");
            // printTextFB(x,y,temp);
            write_string(temps, APPLY_HDEADBAND(GRAPHICS_RIGHT / 2), (GRAPHICS_BOTTOM / 2), 0, 0, TEXT_VA_TOP, TEXT_HA_CENTER, 0, 3);
        }

        char temp[50] =
        { 0 };
        memset(temp, ' ', 40);
        // Note: cast to double required due to -Wdouble-promotion compiler option is
        // being used, and there is no way in C to pass a float to a variadic function like sprintf()
        sprintf(temp, "Lat:%11.7f", (double)(gpsData.Latitude / 10000000.0f));
        write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM - 30), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3);
        sprintf(temp, "Lon:%11.7f", (double)(gpsData.Longitude / 10000000.0f));
        write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM - 10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3);
        sprintf(temp, "Sat:%d", (int)gpsData.Satellites);
        write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT - 40), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);

        /* Print ADC voltage FLIGHT*/
        sprintf(temp, "V:%5.2fV", (double)(PIOS_ADC_PinGet(2) * 3 * 6.1f / 4096));
        write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(20), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);

        if (gpsData.Heading > 180) {
            calcHomeArrow((int16_t)(gpsData.Heading - 360));
        } else {
            calcHomeArrow((int16_t)(gpsData.Heading));
        }
    }
    break;
    case 1:
    {
        /*drawBox(2,2,GRAPHICS_WIDTH_REAL-4,GRAPHICS_HEIGHT_REAL-4);
           write_filled_rectangle(draw_buffer_mask,0,0,GRAPHICS_WIDTH_REAL-2,GRAPHICS_HEIGHT_REAL-2,0);
           write_filled_rectangle(draw_buffer_mask,2,2,GRAPHICS_WIDTH_REAL-4-2,GRAPHICS_HEIGHT_REAL-4-2,2);
           write_filled_rectangle(draw_buffer_mask,3,3,GRAPHICS_WIDTH_REAL-4-1,GRAPHICS_HEIGHT_REAL-4-1,0);*/
        // write_filled_rectangle(draw_buffer_mask,5,5,GRAPHICS_WIDTH_REAL-4-5,GRAPHICS_HEIGHT_REAL-4-5,0);
        // write_rectangle_outlined(10,10,GRAPHICS_WIDTH_REAL-20,GRAPHICS_HEIGHT_REAL-20,0,0);
        // drawLine(GRAPHICS_WIDTH_REAL-1, GRAPHICS_HEIGHT_REAL-1,(GRAPHICS_WIDTH_REAL/2)-1, GRAPHICS_HEIGHT_REAL-1 );
        // drawCircle((GRAPHICS_WIDTH_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1);
        // drawCircle((GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-2);
        // drawLine(0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1, (GRAPHICS_SIZE/2)-1);
        // drawLine((GRAPHICS_SIZE/2)-1, 0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1);
        /*angleA++;
           if(angleB<=-90)
           {
           sum=2;
           }
           if(angleB>=90)
           {
           sum=-2;
           }
           angleB+=sum;
           angleC+=2;*/

        // GPS HACK
        if (gpsData.Heading > 180) {
            calcHomeArrow((int16_t)(gpsData.Heading - 360));
        } else {
            calcHomeArrow((int16_t)(gpsData.Heading));
        }

        /* Draw Attitude Indicator */
        if (OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) {
            drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup.X),
                         APPLY_VDEADBAND(OsdSettings.AttitudeSetup.Y), attitude.Pitch, attitude.Roll, 96);
        }
        // write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0);
        // printText16( 60, 12,"Hello OP-OSD");

        char temp[50] =
        { 0 };
        memset(temp, ' ', 40);
        sprintf(temp, "Lat:%11.7f", (double)(gpsData.Latitude / 10000000.0f));
        write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
        sprintf(temp, "Lon:%11.7f", (double)(gpsData.Longitude / 10000000.0f));
        write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
        sprintf(temp, "Fix:%d", (int)gpsData.Status);
        write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
        sprintf(temp, "Sat:%d", (int)gpsData.Satellites);
        write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);

        /* Print RTC time */
        if (OsdSettings.Time == OSDSETTINGS_TIME_ENABLED) {
            printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup.X), APPLY_VDEADBAND(OsdSettings.TimeSetup.Y));
        }

        /* Print Number of detected video Lines */
        sprintf(temp, "Lines:%4d", PIOS_Video_GetOSDLines());
        write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);

        /* Print ADC voltage */
        // sprintf(temp,"Rssi:%4dV",(int)(PIOS_ADC_PinGet(4)*3000/4096));
        // write_string(temp, (GRAPHICS_WIDTH_REAL - 2),15, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
        sprintf(temp, "Rssi:%4.2fV", (double)(PIOS_ADC_PinGet(5) * 3.0f / 4096.0f));
        write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);

        /* Print CPU temperature */
        sprintf(temp, "Temp:%4.2fC", (double)(PIOS_ADC_PinGet(3) * 0.29296875f - 264));
        write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);

        /* Print ADC voltage FLIGHT*/
        sprintf(temp, "FltV:%4.2fV", (double)(PIOS_ADC_PinGet(2) * 3.0f * 6.1f / 4096.0f));
        write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);

        /* Print ADC voltage VIDEO*/
        sprintf(temp, "VidV:%4.2fV", (double)(PIOS_ADC_PinGet(4) * 3.0f * 6.1f / 4096.0f));
        write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(45), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);

        /* Print ADC voltage RSSI */
        // sprintf(temp,"Curr:%4dA",(int)(PIOS_ADC_PinGet(0)*300*61/4096));
        // write_string(temp, (GRAPHICS_WIDTH_REAL - 2),60, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
        /* Draw Battery Gauge */
        /*m_batt++;
           uint8_t dir=3;
           if(m_batt==101)
           m_batt=0;
           if(m_pitch>0)
           {
           dir=0;
           m_alt+=m_pitch/2;
           }
           else if(m_pitch<0)
           {
           dir=1;
           m_alt+=m_pitch/2;
           }*/

        /*if(OsdSettings.Battery == OSDSETTINGS_BATTERY_ENABLED)
           {
           drawBattery(APPLY_HDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_X]),APPLY_VDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_Y]),m_batt,16);
           }*/

        // drawAltitude(200,50,m_alt,dir);
        // drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32);
        // Draw airspeed (left side.)
        if (OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) {
            hud_draw_vertical_scale((int)gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup.X),
                                    APPLY_VDEADBAND(OsdSettings.SpeedSetup.Y), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE);
        }
        // Draw altimeter (right side.)
        if (OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) {
            hud_draw_vertical_scale((int)gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup.X),
                                    APPLY_VDEADBAND(OsdSettings.AltitudeSetup.Y), 100, 20, 100, 7, 12, 15, 500, 0);
        }
        // Draw compass.
        if (OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) {
            if (attitude.Yaw < 0) {
                hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup.X),
                                        APPLY_VDEADBAND(OsdSettings.HeadingSetup.Y), 15, 30, 7, 12, 0);
            } else {
                hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup.X),
                                        APPLY_VDEADBAND(OsdSettings.HeadingSetup.Y), 15, 30, 7, 12, 0);
            }
        }
    }
    break;
    case 2:
    {
        int size = 64;
        int x    = ((GRAPHICS_RIGHT / 2) - (size / 2)), y = (GRAPHICS_BOTTOM - size - 2);
        draw_artificial_horizon(-attitude.Roll, attitude.Pitch, APPLY_HDEADBAND(x), APPLY_VDEADBAND(y), size);
        hud_draw_vertical_scale((int)gpsData.Groundspeed, 20, +1, APPLY_HDEADBAND(GRAPHICS_RIGHT - (x - 1)), APPLY_VDEADBAND(y + (size / 2)), size, 5, 10, 4, 7,
                                10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE);
        if (OsdSettings.AltitudeSource == OSDSETTINGS_ALTITUDESOURCE_BARO) {
            hud_draw_vertical_scale((int)baro.Altitude, 50, -1, APPLY_HDEADBAND((x + size + 1)), APPLY_VDEADBAND(y + (size / 2)), size, 10, 20, 4, 7, 10, 500, 0);
        } else {
            hud_draw_vertical_scale((int)gpsData.Altitude, 50, -1, APPLY_HDEADBAND((x + size + 1)), APPLY_VDEADBAND(y + (size / 2)), size, 10, 20, 4, 7, 10, 500,
                                    0);
        }

        char temp[50] =
        { 0 };
        memset(temp, ' ', 50);
        switch (status.FlightMode) {
        case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
            sprintf(temp, "Man");
            break;
        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
            sprintf(temp, "Stab1");
            break;
        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
            sprintf(temp, "Stab2");
            break;
        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
            sprintf(temp, "Stab3");
            break;
        case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
            sprintf(temp, "PH");
            break;
        case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
            sprintf(temp, "RTB");
            break;
        case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
            sprintf(temp, "PATH");
            break;
        default:
            sprintf(temp, "Mode: %d", status.FlightMode);
            break;
        }
        write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
    }
    break;
    case 3:
    {
        lamas();
    }
    break;
    case 4:
    case 5:
    case 6:
    {
        int image = OsdSettings.Screen - 4;
        struct splashEntry splash_info;
        splash_info = splash[image];

        copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - (splash_info.width) / 2), APPLY_VDEADBAND(GRAPHICS_BOTTOM / 2 - (splash_info.height) / 2), image);
    }
    break;
    default:
        write_vline_lm(APPLY_HDEADBAND(GRAPHICS_RIGHT / 2), APPLY_VDEADBAND(0), APPLY_VDEADBAND(GRAPHICS_BOTTOM), 1, 1);
        write_hline_lm(APPLY_HDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT), APPLY_VDEADBAND(GRAPHICS_BOTTOM / 2), 1, 1);
        break;
    }

    // Must mask out last half-word because SPI keeps clocking it out otherwise
    for (uint32_t i = 0; i < 8; i++) {
        write_vline(draw_buffer_level, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
        write_vline(draw_buffer_mask, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
    }
}

void updateOnceEveryFrame()
{
    clearGraphics();
    updateGraphics();
}

// ****************
/**
 * Initialise the gps module
 * \return -1 if initialisation failed
 * \return 0 on success
 */

int32_t osdgenStart(void)
{
    // Start gps task
    vSemaphoreCreateBinary(osdSemaphore);
    xTaskCreate(osdgenTask, "OSDGEN", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdgenTaskHandle);
    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_OSDGEN, osdgenTaskHandle);
#ifdef PIOS_INCLUDE_WDG
    PIOS_WDG_RegisterFlag(PIOS_WDG_OSDGEN);
#endif
    return 0;
}

/**
 * Initialise the osd module
 * \return -1 if initialisation failed
 * \return 0 on success
 */
int32_t osdgenInitialize(void)
{
    AttitudeStateInitialize();
#ifdef PIOS_INCLUDE_GPS
    GPSPositionSensorInitialize();
#if !defined(PIOS_GPS_MINIMAL)
    GPSTimeInitialize();
    GPSSatellitesInitialize();
#endif
#ifdef PIOS_GPS_SETS_HOMELOCATION
    HomeLocationInitialize();
#endif
#endif
    OsdSettingsInitialize();
    BaroSensorInitialize();
    FlightStatusInitialize();

    return 0;
}
MODULE_INITCALL(osdgenInitialize, osdgenStart);

// ****************
/**
 * Main osd task. It does not return.
 */

static void osdgenTask(__attribute__((unused)) void *parameters)
{
    // portTickType lastSysTime;
    // Loop forever
    // lastSysTime = xTaskGetTickCount();
    OsdSettingsData OsdSettings;

    OsdSettingsGet(&OsdSettings);

    PIOS_Servo_Set(0, OsdSettings.White);
    PIOS_Servo_Set(1, OsdSettings.Black);

    // intro
    for (int i = 0; i < 63; i++) {
        if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) {
#ifdef PIOS_INCLUDE_WDG
            PIOS_WDG_UpdateFlag(PIOS_WDG_OSDGEN);
#endif
            clearGraphics();
            introGraphics();
        }
    }
    for (int i = 0; i < 63; i++) {
        if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) {
#ifdef PIOS_INCLUDE_WDG
            PIOS_WDG_UpdateFlag(PIOS_WDG_OSDGEN);
#endif
            clearGraphics();
            introGraphics();
            introText();
        }
    }

    while (1) {
        if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) {
#ifdef PIOS_INCLUDE_WDG
            PIOS_WDG_UpdateFlag(PIOS_WDG_OSDGEN);
#endif
            updateOnceEveryFrame();
        }
        // xSemaphoreTake(osdSemaphore, portMAX_DELAY);
        // vTaskDelayUntil(&lastSysTime, 10 / portTICK_RATE_MS);
    }
}

// ****************

/**
 * @}
 * @}
 */