diff --git a/apps/gui/color_picker.c b/apps/gui/color_picker.c
index 1217fcd..0665397 100644
--- a/apps/gui/color_picker.c
+++ b/apps/gui/color_picker.c
@@ -23,6 +23,7 @@
 #include "kernel.h"
 #include "system.h"
 #include "screen_access.h"
+#include "font.h"
 #include "debug.h"
 #include "misc.h"
 #include "settings.h"
@@ -31,115 +32,291 @@
 #include "splash.h"
 #include "action.h"
 
-#define TEXT_MARGIN display->char_width+2
-#define SLIDER_START 20
+/* structure for color info */
+struct rgb_pick
+{
+    unsigned color;                 /* native color value            */
+    union
+    {
+        unsigned char rgb_val[6];   /* access to components as array */
+        struct
+        {
+            unsigned char r;        /* native red value              */
+            unsigned char g;        /* native green value            */
+            unsigned char b;        /* native blue value             */
+            unsigned char red;      /* 8 bit red value               */
+            unsigned char green;    /* 8 bit green value             */
+            unsigned char blue;     /* 8 bit blue value              */
+        } __attribute__ ((__packed__)); /* assume byte packing       */
+    };
+};
 
-static const int max_val[3] = {LCD_MAX_RED,LCD_MAX_GREEN,LCD_MAX_BLUE};
+/* maximum values for components */
+static const unsigned char max_val[3] =
+{
+    LCD_MAX_RED,
+    LCD_MAX_GREEN,
+    LCD_MAX_BLUE
+};
+
+/* list of primary colors */
+static const unsigned prim_rgb[3] =
+{
+    LCD_RGBPACK(255, 0, 0),
+    LCD_RGBPACK(0, 255, 0),
+    LCD_RGBPACK(0, 0 ,255),
+};
+
+/* Unpacks the color value into native rgb values and 24 bit rgb values */
+static void unpack_rgb(struct rgb_pick *rgb)
+{
+    unsigned color = rgb->color;
+#if LCD_PIXELFORMAT == RGB565SWAPPED
+    swap16(color);
+#endif
+    rgb->r     = (color & 0xf800) >> 11;
+    rgb->g     = (color & 0x07e0) >> 5;
+    rgb->b     = (color & 0x001f);
+    rgb->red   = _RGB_UNPACK_RED(color);
+    rgb->green = _RGB_UNPACK_GREEN(color);
+    rgb->blue  = _RGB_UNPACK_BLUE(color);
+}
+
+/* Packs the native rgb colors into a color value */
+static void pack_rgb(struct rgb_pick *rgb)
+{
+    unsigned color = (rgb->r & 0x1f) << 11 |
+                     (rgb->g & 0x3f) << 5 |
+                     (rgb->b & 0x1f);
+#if LCD_PIXELFORMAT == RGB565SWAPPED
+    swap16(color);
+#endif
+    rgb->color = color;
+}
+
+/* Returns LCD_BLACK if the color is above a threshold brightness
+   else return LCD_WHITE */
+static inline unsigned get_black_or_white(const struct rgb_pick *rgb)
+{
+    return (4*rgb->r + 5*rgb->g + 2*rgb->b) >= 256 ?
+        LCD_BLACK : LCD_WHITE;
+}
+
+#define MARGIN_LEFT             2 /* Left margin of screen                */
+#define MARGIN_TOP              4 /* Top margin of screen                 */
+#define MARGIN_RIGHT            2 /* Right margin of screen               */
+#define MARGIN_BOTTOM           4 /* Bottom margin of screen              */
+#define SLIDER_MARGIN_LEFT      2 /* Gap to left of sliders               */
+#define SLIDER_MARGIN_RIGHT     2 /* Gap to right of sliders              */
+#define TITLE_MARGIN_BOTTOM     4 /* Space below title bar                */
+#define SELECTOR_LR_MARGIN      1 /* Margin between ">" and text          */
+#define SELECTOR_TB_MARGIN      1 /* Margin on top and bottom of selector */
+#define SWATCH_TOP_MARGIN       4 /* Space between last slider and swatch */
+
+/* dunno why lcd_set_drawinfo should be left out of struct screen */
+static void set_drawinfo(struct screen *display, int mode,
+                         unsigned foreground, unsigned background)
+{
+    display->set_drawmode(mode);
+    if (display->depth > 1)
+    {
+        display->set_foreground(foreground);
+        display->set_background(background);
+    }
+}
 
 static void draw_screen(struct screen *display, char *title,
-                        int *rgb_val, int color, int row)
+                        struct rgb_pick *rgb, int row)
 {
-    int i;
-    char *textrgb = str(LANG_COLOR_RGB_LABELS), rgb_dummy[2] = {0,0};
-    int text_top;
-    int text_centre, bg_col;
-    char buf[32];
-    int slider_width = (display->width-SLIDER_START-(display->char_width*5));
-    int background_color = global_settings.bg_color;
-    int text_color = global_settings.fg_color;
-    bool display_three_rows = (display->height/6) > display->char_height;
+    unsigned  text_color       = LCD_BLACK;
+    unsigned  background_color = LCD_WHITE;
+    char      buf[32];
+    int       i, x, y;
+    int       text_top;
+    int       slider_left, slider_width;
+    bool      display_three_rows;
+    int       max_label_width;
 
     display->clear_display();
 
-    if (display->depth > 1) {
-        display->set_foreground(text_color);
+    if (display->depth > 1)
+    {
+        text_color       = display->get_foreground();
+        background_color = display->get_background();
     }
 
-    i = display->getstringsize(title,0,0);
-    display->putsxy((display->width-i)/2,6,title );
+    /* Find out if there's enought room for three slider or just
+       enought to display the selected slider */
+    display_three_rows = display->height >= display->char_height*4 +
+                                            MARGIN_TOP +
+                                            TITLE_MARGIN_BOTTOM +
+                                            MARGIN_BOTTOM;
 
-    text_top = display->char_height*2;
-    bg_col = background_color;
-    for (i=0; i<3 ;i++)
+    /* Figure out widest label character in case they vary */
+    for (i = 0, max_label_width = 0; i < 3; i++)
     {
+        buf[0] = str(LANG_COLOR_RGB_LABELS)[i];
+        buf[1] = '\0';
+        x = display->getstringsize(buf, NULL, NULL);
+        if (x > max_label_width)
+            max_label_width = x;
+    }
+
+    /* Draw title string */
+    set_drawinfo(display, DRMODE_SOLID, text_color, background_color);
+    display->getstringsize(title, &x, &y);
+    display->putsxy((display->width - x) / 2, MARGIN_TOP, title);
+
+    /* Get slider positions and top starting position */
+    text_top     = MARGIN_TOP + y + TITLE_MARGIN_BOTTOM + SELECTOR_TB_MARGIN;
+    slider_left  = MARGIN_LEFT + SELECTOR_LR_MARGIN + display->char_width +
+                   max_label_width + SLIDER_MARGIN_LEFT;
+    slider_width = display->width - slider_left - SLIDER_MARGIN_RIGHT -
+                   SELECTOR_LR_MARGIN - display->char_width*3 - MARGIN_RIGHT;
+
+    for (i = 0; i < 3; i++)
+    {
+        int      mode = DRMODE_SOLID;
+        unsigned fg   = text_color;
+        unsigned bg   = background_color;
+
         if (!display_three_rows)
             i = row;        
 
-        if (i==row)
+        if (i == row)
         {
-            if ((global_settings.invert_cursor) && (display->depth >2))
+            set_drawinfo(display, DRMODE_SOLID, text_color,
+                         background_color);
+
+            if (global_settings.invert_cursor)
             {
-                display->fillrect(0,text_top-1,display->width,display->char_height+2);
-                bg_col = text_color;
+                /* Draw solid bar selection bar */
+                display->fillrect(0,
+                                  text_top - SELECTOR_TB_MARGIN,
+                                  display->width,
+                                  display->char_height +
+                                  SELECTOR_TB_MARGIN*2);
+
+                if (display->depth == 1)
+                {
+                    /* Just invert for low mono display */
+                    mode |= DRMODE_INVERSEVID;
+                }
+                else
+                {
+                    if (display->depth >= 16)
+                    {
+                        /* Backdrops will show through text in
+                           DRMODE_SOLID */
+                        mode = DRMODE_FG;
+                        fg = prim_rgb[i];
+                    }
+                    else
+                    {
+                        fg = background_color;
+                    }
+
+                    bg = text_color;
+                }
             }
             else if (display_three_rows)
             {
-                display->putsxy(0,text_top,">");
-                display->putsxy(display->width-display->char_width-2,text_top,"<");
-                bg_col = background_color;
-            }
-            if (display->depth > 1)
-            {
-                if (i==0)
-                    display->set_foreground(LCD_RGBPACK(255, 0, 0));
-                else if (i==1)
-                    display->set_foreground(LCD_RGBPACK(0, 255, 0));
-                else if (i==2)
-                    display->set_foreground(LCD_RGBPACK(0 ,0 ,255));
+                /* Draw ">    <" around sliders */
+                display->putsxy(MARGIN_LEFT,  text_top, ">");
+                display->putsxy(display->width-display->char_width -
+                                MARGIN_RIGHT, text_top, "<");
+                if (display->depth >= 16)
+                    fg = prim_rgb[i];
             }
         }
-        else
-        {
-            if (display->depth > 1)
-                display->set_foreground(text_color);
-            bg_col = background_color;
-        }
+ 
+        set_drawinfo(display, mode, fg, bg);
 
-        if (display->depth > 1)
-            display->set_background(bg_col);
+        /* Draw label - assumes labels are one character */
+        buf[0] = str(LANG_COLOR_RGB_LABELS)[i];
+        buf[1] = '\0';
+        display->putsxy(slider_left - display->char_width -
+                        SLIDER_MARGIN_LEFT, text_top, buf);
 
-        text_centre = text_top+(display->char_height/2);
-        rgb_dummy[0] = textrgb[i];
-        display->putsxy(TEXT_MARGIN,text_top,rgb_dummy);
-        snprintf(buf,3,"%02d",rgb_val[i]);
-        display->putsxy(display->width-(display->char_width*4),text_top,buf);
+        /* Draw color value */
+        snprintf(buf, 3, "%02d", rgb->rgb_val[i]);
+        display->putsxy(slider_left + slider_width + SLIDER_MARGIN_RIGHT,
+                        text_top, buf);
 
-        text_top += display->char_height/4;
+        /* Draw scrollbar */
+        gui_scrollbar_draw(display,
+                           slider_left,
+                           text_top + display->char_height / 4,
+                           slider_width,
+                           display->char_height / 2,
+                           max_val[i],
+                           0,
+                           rgb->rgb_val[i],
+                           HORIZONTAL);
 
-        gui_scrollbar_draw(display,SLIDER_START,text_top,slider_width,
-                           display->char_height/2,
-                           max_val[i],0,rgb_val[i],HORIZONTAL);
+        /* Advance to next line */
+        text_top += display->char_height + 2*SELECTOR_TB_MARGIN;
+
         if (!display_three_rows)
-           break;
-        text_top += display->char_height;
+            break;
     }
 
-    if (display->depth > 1) {
-        display->set_background(background_color);
-        display->set_foreground(text_color);
+    /* Format RGB: #rrggbb */
+    snprintf(buf, sizeof(buf), str(LANG_COLOR_RGB_VALUE),
+                               rgb->red, rgb->green, rgb->blue);
+
+    if (display->depth >= 16)
+    {
+        /* Display color swatch on color screens only */
+        int left   = slider_left;
+        int top    = text_top + SWATCH_TOP_MARGIN;
+        int width  = display->width - slider_left - left;
+        int height = display->height - top - MARGIN_BOTTOM;
+
+        display->setfont(FONT_SYSFIXED);
+
+        /* Only draw if room */
+        if (height >= display->char_height + 2)
+        {
+            display->set_foreground(rgb->color);
+            display->fillrect(left, top, width, height);
+
+            /* Draw RGB: #rrggbb in middle of swatch */
+            display->set_drawmode(DRMODE_FG);
+            display->getstringsize(buf, &x, &y);
+            display->set_foreground(get_black_or_white(rgb));
+
+            x = left + (width  - x) / 2;
+            y = top  + (height - y) / 2;
+
+            display->putsxy(x, y, buf);
+            display->set_drawmode(DRMODE_SOLID);
+
+            /* Draw border */
+            display->set_foreground(text_color);
+            display->drawrect(left - 1, top - 1, width + 2, height + 2);
+        }
+
+        display->setfont(FONT_UI);
     }
+    else
+    {
+        /* Display RGB value only centered on remaining display if room */
+        display->getstringsize(buf, &x, &y);
+        i = text_top + SWATCH_TOP_MARGIN;
 
-    if (text_top + (display->char_height*2) < (display->height-40-display->char_height))
-        text_top += (display->char_height*2);
-    else text_top += (display->char_height);
-
-    /* Display RGB: #rrggbb */
-    snprintf(buf,sizeof(buf),str(LANG_COLOR_RGB_VALUE),
-                                 RGB_UNPACK_RED(color),
-                                 RGB_UNPACK_GREEN(color),
-                                 RGB_UNPACK_BLUE(color));
-
-    display->putsxy((display->width-(display->char_width*11))/2,text_top,buf);
-
-    if (display->depth > 1) {
-        display->set_foreground(color);
-        display->fillrect(SLIDER_START,LCD_HEIGHT-40,slider_width,35);
-
-        display->set_foreground(LCD_BLACK);
-        display->drawrect(SLIDER_START-1,LCD_HEIGHT-41,slider_width+2,37);
+        if (i + y <= display->height - MARGIN_BOTTOM)
+        {
+            set_drawinfo(display, DRMODE_SOLID, text_color, background_color);
+            x = (display->width - x) / 2;
+            y = (i + display->height - MARGIN_BOTTOM - y) / 2;
+            display->putsxy(x, y, buf);
+        }
     }
 
     display->update();
+    /* Be sure screen mode is reset */
+    set_drawinfo(display, DRMODE_SOLID, text_color, background_color);
 }
 
 /***********
@@ -148,74 +325,59 @@
  color is a pointer to the colour (in native format) to modify
  set banned_color to -1 to allow all
  ***********/
-bool set_color(struct screen *display,char *title, int* color, int banned_color)
+bool set_color(struct screen *display, char *title, int* color, int banned_color)
 {
-    int exit = 0, button, slider=0;
-    int rgb_val[3]; /* native depth r,g,b*/;
-    int fgcolor = display->get_foreground();
-    int newcolor = *color;
+    int exit = 0, button, slider = 0;
     int i;
+    struct rgb_pick rgb;
+    (void)display;
 
-#if LCD_PIXELFORMAT == RGB565
-    rgb_val[0] = ((*color)&0xf800) >> 11;
-    rgb_val[1] = ((*color)&0x07e0) >> 5;
-    rgb_val[2] = ((*color)&0x001f);
-#elif LCD_PIXELFORMAT == RGB565SWAPPED
-    rgb_val[0] = ((swap16(*color))&0xf800) >> 11;
-    rgb_val[1] = ((swap16(*color))&0x07e0) >> 5;
-    rgb_val[2] = ((swap16(*color))&0x001f);
-#endif
+    rgb.color = *color;
 
     while (!exit)
     {
-        /* We need to maintain three versions of the colour:
+        unpack_rgb(&rgb);
 
-           rgb_val[3] - the native depth RGB values
-           newcolor - the native format packed colour
-         */
-
-#if LCD_PIXELFORMAT == RGB565
-        newcolor = (rgb_val[0] << 11) | (rgb_val[1] << 5) | (rgb_val[2]);
-#elif LCD_PIXELFORMAT == RGB565SWAPPED
-        newcolor = swap16((rgb_val[0] << 11) | (rgb_val[1] << 5) | (rgb_val[2]));
-#endif
         FOR_NB_SCREENS(i)
         {
-            draw_screen(&screens[i], title, rgb_val, newcolor, slider);
+            draw_screen(&screens[i], title, &rgb, slider);
         }
-        
-        button = get_action(CONTEXT_SETTINGS_COLOURCHOOSER,TIMEOUT_BLOCK);
+
+        button = get_action(CONTEXT_SETTINGS_COLOURCHOOSER, TIMEOUT_BLOCK);
+
         switch (button)
         {
             case ACTION_STD_PREV:
             case ACTION_STD_PREVREPEAT:
-                slider = (slider+2)%3;
+                slider = (slider + 2) % 3;
                 break;
 
             case ACTION_STD_NEXT:
             case ACTION_STD_NEXTREPEAT:
-                slider = (slider+1)%3;
+                slider = (slider + 1) % 3;
                 break;
 
             case ACTION_SETTINGS_INC:
             case ACTION_SETTINGS_INCREPEAT:
-                if (rgb_val[slider] < max_val[slider])
-                    rgb_val[slider]++;
+                if (rgb.rgb_val[slider] < max_val[slider])
+                    rgb.rgb_val[slider]++;
+                pack_rgb(&rgb);
                 break;
 
             case ACTION_SETTINGS_DEC:
             case ACTION_SETTINGS_DECREPEAT:
-                if (rgb_val[slider] > 0)
-                    rgb_val[slider]--;
+                if (rgb.rgb_val[slider] > 0)
+                    rgb.rgb_val[slider]--;
+                pack_rgb(&rgb);
                 break;
 
             case ACTION_STD_OK:
-                if ((banned_color!=-1) && (banned_color == newcolor))
+                if (banned_color != -1 && (unsigned)banned_color == rgb.color)
                 {
-                    gui_syncsplash(HZ*2,true,str(LANG_COLOR_UNACCEPTABLE));
+                    gui_syncsplash(HZ*2, true, str(LANG_COLOR_UNACCEPTABLE));
                     break;
                 }
-                *color = newcolor;
+                *color = rgb.color;
                 exit = 1;
                 break;
 
@@ -224,14 +386,12 @@
                 break;
 
             default:
-                if(default_event_handler(button) == SYS_USB_CONNECTED) {
-                    display->set_foreground(fgcolor);
+                if (default_event_handler(button) == SYS_USB_CONNECTED)
                     return true;
-                }
                 break;
         }
     }
-    display->set_foreground(fgcolor);
+
     action_signalscreenchange();
     return false;
 }