WIP base watch system.

src/twali_power.cpp

@@ -141,7 +141,7 @@ void twali_power_suspend(struct twali_power *p) {
     p->system->tft->fillScreen(TFT_BLACK);
     esp_light_sleep_start();
     twali_system_wake_up(p->system);
-    printf("%s\n", "back from sleep");
+    printf("%s\n", "back from suspend");
 }
 
 void twali_power_task(void * object);

src/twali_power.h

@@ -7,6 +7,8 @@
 extern "C" {
 #endif
 
+#define TWALI_POWER_TOPIC "POWER"
+
 struct twali_power;
 struct twali_task;
 

src/twali_screen.cpp

@@ -0,0 +1,321 @@
+
+struct twali_screen {
+    int x; 
+    int y; 
+    int w; 
+    int h;
+    TFT_eSPI * tft;
+};
+
+void twali_screen_clip(struct twali_screen * s, int32_t * x, int32_t * y) {
+    int cx = *x + s->x;
+    int cy = *y + s->y;
+    if cx > (    
+    
+}
+
+void twali_screen_draw_pixel(struct twali_screen * s, int32_t x, int32_t y, uint32_t color) {
+    s->tft->drawPixel(x, y, color);
+}
+
+void twali_screen_draw_char(struct twali_screen * s, int32_t x, int32_t y, uint16_t c, uint32_t color, uint32_t bg, uint8_t size) {
+    s->tft->drawChar(x, y, c, color, bg, size);
+}
+
+void twali_screen_draw_line(struct twali_screen * s, int32_t x, int32_t y, int32_t w, int32_t h, uint32_t color) {
+    s->tft->drawLine(x, y, x+w, y+h, c, color, bg, size);
+}
+
+            drawChar(int32_t x, int32_t y, uint16_t c, uint32_t color, uint32_t bg, uint8_t size),
+            drawLine(int32_t xs, int32_t ys, int32_t xe, int32_t ye, uint32_t color),
+            drawFastVLine(int32_t x, int32_t y, int32_t h, uint32_t color),
+            drawFastHLine(int32_t x, int32_t y, int32_t w, uint32_t color),
+            fillRect(int32_t x, int32_t y, int32_t w, int32_t h, uint32_t color);
+
+    virtual int16_t  drawChar(uint16_t uniCode, int32_t x, int32_t y, uint8_t font),
+            drawChar(uint16_t uniCode, int32_t x, int32_t y),
+            height(void),
+            width(void);
+
+    void     setRotation(uint8_t r); // Set the display image orientation to 0, 1, 2 or 3
+    uint8_t  getRotation(void);      // Read the current rotation
+
+    void     invertDisplay(bool i);  // Tell TFT to invert all displayed colours
+
+
+    // The TFT_eSprite class inherits the following functions (not all are useful to Sprite class
+    void     setAddrWindow(int32_t xs, int32_t ys, int32_t w, int32_t h), // Note: start coordinates + width and height
+             setWindow(int32_t xs, int32_t ys, int32_t xe, int32_t ye);   // Note: start + end coordinates
+
+    // Push (aka write pixel) colours to the TFT (use setAddrWindow() first)
+    void     pushColor(uint16_t color),
+             pushColor(uint16_t color, uint32_t len),  // Deprecated, use pushBlock()
+             pushColors(uint16_t  *data, uint32_t len, bool swap = true), // With byte swap option
+             pushColors(uint8_t  *data, uint32_t len); // Deprecated, use pushPixels()
+
+    // Write a solid block of a single colour
+    void     pushBlock(uint16_t color, uint32_t len);
+
+    // Write a set of pixels stored in memory, use setSwapBytes(true/false) function to correct endianess
+    void     pushPixels(const void *data_in, uint32_t len);
+
+    // Read the colour of a pixel at x,y and return value in 565 format
+    uint16_t readPixel(int32_t x, int32_t y);
+
+    // Support for half duplex (bi-directional SDA) SPI bus where MOSI must be switched to input
+#ifdef TFT_SDA_READ
+#if defined (TFT_eSPI_ENABLE_8_BIT_READ)
+    uint8_t  tft_Read_8(void);     // Read 8 bit value from TFT command register
+#endif
+    void     begin_SDA_Read(void); // Begin a read on a half duplex (bi-directional SDA) SPI bus - sets MOSI to input
+    void     end_SDA_Read(void);   // Restore MOSI to output
+#endif
+
+    // Graphics drawing
+    void     fillScreen(uint32_t color),
+             drawRect(int32_t x, int32_t y, int32_t w, int32_t h, uint32_t color),
+             drawRoundRect(int32_t x, int32_t y, int32_t w, int32_t h, int32_t radius, uint32_t color),
+             fillRoundRect(int32_t x, int32_t y, int32_t w, int32_t h, int32_t radius, uint32_t color);
+
+
+    void     drawCircle(int32_t x, int32_t y, int32_t r, uint32_t color),
+             drawCircleHelper(int32_t x, int32_t y, int32_t r, uint8_t cornername, uint32_t color),
+             fillCircle(int32_t x, int32_t y, int32_t r, uint32_t color),
+             fillCircleHelper(int32_t x, int32_t y, int32_t r, uint8_t cornername, int32_t delta, uint32_t color),
+
+             drawEllipse(int16_t x, int16_t y, int32_t rx, int32_t ry, uint16_t color),
+             fillEllipse(int16_t x, int16_t y, int32_t rx, int32_t ry, uint16_t color),
+
+             //                 Corner 1               Corner 2               Corner 3
+             drawTriangle(int32_t x1, int32_t y1, int32_t x2, int32_t y2, int32_t x3, int32_t y3, uint32_t color),
+             fillTriangle(int32_t x1, int32_t y1, int32_t x2, int32_t y2, int32_t x3, int32_t y3, uint32_t color);
+
+    // Image rendering
+    // Swap the byte order for pushImage() and pushPixels() - corrects endianness
+    void     setSwapBytes(bool swap);
+    bool     getSwapBytes(void);
+
+    // Draw bitmap
+    void     drawBitmap( int16_t x, int16_t y, const uint8_t *bitmap, int16_t w, int16_t h, uint16_t fgcolor),
+             drawBitmap( int16_t x, int16_t y, const uint8_t *bitmap, int16_t w, int16_t h, uint16_t fgcolor, uint16_t bgcolor),
+             drawXBitmap(int16_t x, int16_t y, const uint8_t *bitmap, int16_t w, int16_t h, uint16_t fgcolor),
+             drawXBitmap(int16_t x, int16_t y, const uint8_t *bitmap, int16_t w, int16_t h, uint16_t fgcolor, uint16_t bgcolor),
+             setBitmapColor(uint16_t fgcolor, uint16_t bgcolor); // Define the 2 colours for 1bpp sprites
+
+    // Set TFT pivot point (use when rendering rotated sprites)
+    void     setPivot(int16_t x, int16_t y);
+    int16_t  getPivotX(void), // Get pivot x
+             getPivotY(void); // Get pivot y
+
+    // The next functions can be used as a pair to copy screen blocks (or horizontal/vertical lines) to another location
+    // Read a block of pixels to a data buffer, buffer is 16 bit and the size must be at least w * h
+    void     readRect(int32_t x, int32_t y, int32_t w, int32_t h, uint16_t *data);
+    // Write a block of pixels to the screen which have been read by readRect()
+    void     pushRect(int32_t x, int32_t y, int32_t w, int32_t h, uint16_t *data);
+
+    // These are used to render images or sprites stored in RAM arrays (used by Sprite class for 16bpp Sprites)
+    void     pushImage(int32_t x, int32_t y, int32_t w, int32_t h, uint16_t *data);
+    void     pushImage(int32_t x, int32_t y, int32_t w, int32_t h, uint16_t *data, uint16_t transparent);
+
+    // These are used to render images stored in FLASH (PROGMEM)
+    void     pushImage(int32_t x, int32_t y, int32_t w, int32_t h, const uint16_t *data, uint16_t transparent);
+    void     pushImage(int32_t x, int32_t y, int32_t w, int32_t h, const uint16_t *data);
+
+    // These are used by Sprite class pushSprite() member function for 1, 4 and 8 bits per pixel (bpp) colours
+    // They are not intended to be used with user sketches (but could be)
+    // Set bpp8 true for 8bpp sprites, false otherwise. The cmap pointer must be specified for 4bpp
+    void     pushImage(int32_t x, int32_t y, int32_t w, int32_t h, uint8_t  *data, bool bpp8 = true, uint16_t *cmap = nullptr);
+    void     pushImage(int32_t x, int32_t y, int32_t w, int32_t h, uint8_t  *data, uint8_t  transparent, bool bpp8 = true, uint16_t *cmap = nullptr);
+
+    // This next function has been used successfully to dump the TFT screen to a PC for documentation purposes
+    // It reads a screen area and returns the 3 RGB 8 bit colour values of each pixel in the buffer
+    // Set w and h to 1 to read 1 pixel's colour. The data buffer must be at least w * h * 3 bytes
+    void     readRectRGB(int32_t x, int32_t y, int32_t w, int32_t h, uint8_t *data);
+
+    // Text rendering - value returned is the pixel width of the rendered text
+    int16_t  drawNumber(long intNumber, int32_t x, int32_t y, uint8_t font), // Draw integer using specified font number
+             drawNumber(long intNumber, int32_t x, int32_t y),               // Draw integer using current font
+
+             // Decimal is the number of decimal places to render
+             // Use with setTextDatum() to position values on TFT, and setTextPadding() to blank old displayed values
+             drawFloat(float floatNumber, uint8_t decimal, int32_t x, int32_t y, uint8_t font), // Draw float using specified font number
+             drawFloat(float floatNumber, uint8_t decimal, int32_t x, int32_t y),               // Draw float using current font
+
+             // Handle char arrays
+             // Use with setTextDatum() to position string on TFT, and setTextPadding() to blank old displayed strings
+             drawString(const char *string, int32_t x, int32_t y, uint8_t font),  // Draw string using specifed font number
+             drawString(const char *string, int32_t x, int32_t y),                // Draw string using current font
+             drawString(const String &string, int32_t x, int32_t y, uint8_t font),// Draw string using specifed font number
+             drawString(const String &string, int32_t x, int32_t y),              // Draw string using current font
+
+             drawCentreString(const char *string, int32_t x, int32_t y, uint8_t font),  // Deprecated, use setTextDatum() and drawString()
+             drawRightString(const char *string, int32_t x, int32_t y, uint8_t font),   // Deprecated, use setTextDatum() and drawString()
+             drawCentreString(const String &string, int32_t x, int32_t y, uint8_t font),// Deprecated, use setTextDatum() and drawString()
+             drawRightString(const String &string, int32_t x, int32_t y, uint8_t font); // Deprecated, use setTextDatum() and drawString()
+
+    // Text rendering and font handling support funtions
+    void     setCursor(int16_t x, int16_t y),                 // Set cursor for tft.print()
+             setCursor(int16_t x, int16_t y, uint8_t font);   // Set cursor and font number for tft.print()
+
+    int16_t  getCursorX(void),                                // Read current cursor x position (moves with tft.print())
+             getCursorY(void);                                // Read current cursor y position
+
+    void     setTextColor(uint16_t color),                    // Set character (glyph) color only (background not over-written)
+             setTextColor(uint16_t fgcolor, uint16_t bgcolor),// Set character (glyph) foreground and backgorund colour
+             setTextSize(uint8_t size);                       // Set character size multiplier (this increases pixel size)
+
+    void     setTextWrap(bool wrapX, bool wrapY = false);     // Turn on/off wrapping of text in TFT width and/or height
+
+    void     setTextDatum(uint8_t datum);                     // Set text datum position (default is top left), see Section 6 above
+    uint8_t  getTextDatum(void);
+
+    void     setTextPadding(uint16_t x_width);                // Set text padding (background blanking/over-write) width in pixels
+    uint16_t getTextPadding(void);                            // Get text padding
+
+#ifdef LOAD_GFXFF
+    void     setFreeFont(const GFXfont *f = NULL),            // Select the GFX Free Font
+             setTextFont(uint8_t font);                       // Set the font number to use in future
+#else
+    void     setFreeFont(uint8_t font),                       // Not used, historical fix to prevent an error
+             setTextFont(uint8_t font);                       // Set the font number to use in future
+#endif
+
+    int16_t  textWidth(const char *string, uint8_t font),     // Returns pixel width of string in specified font
+             textWidth(const char *string),                   // Returns pixel width of string in current font
+             textWidth(const String &string, uint8_t font),   // As above for String types
+             textWidth(const String &string),
+             fontHeight(int16_t font),                        // Returns pixel height of string in specified font
+             fontHeight(void);                                // Returns pixel width of string in current font
+
+    // Used by library and Smooth font class to extract Unicode point codes from a UTF8 encoded string
+    uint16_t decodeUTF8(uint8_t *buf, uint16_t *index, uint16_t remaining),
+             decodeUTF8(uint8_t c);
+
+    // Support function to UTF8 decode and draw characters piped through print stream
+    size_t   write(uint8_t);
+
+    // Used by Smooth font class to fetch a pixel colour for the anti-aliasing
+    void     setCallback(getColorCallback getCol);
+
+    uint16_t fontsLoaded(void); // Each bit in returned value represents a font type that is loaded - used for debug/error handling only
+
+    // Low level read/write
+    void     spiwrite(uint8_t);        // legacy support only
+
+    void     writecommand(uint8_t c),  // Send a command, function resets DC/RS high ready for data
+             writedata(uint8_t d);     // Send data with DC/RS set high
+
+    void     commandList(const uint8_t *addr); // Send a initialisation sequence to TFT stored in FLASH
+
+    uint8_t  readcommand8( uint8_t cmd_function, uint8_t index = 0); // read 8 bits from TFT
+    uint16_t readcommand16(uint8_t cmd_function, uint8_t index = 0); // read 16 bits from TFT
+    uint32_t readcommand32(uint8_t cmd_function, uint8_t index = 0); // read 32 bits from TFT
+
+
+    // Colour conversion
+    // Convert 8 bit red, green and blue to 16 bits
+    uint16_t color565(uint8_t red, uint8_t green, uint8_t blue);
+
+    // Convert 8 bit colour to 16 bits
+    uint16_t color8to16(uint8_t color332);
+    // Convert 16 bit colour to 8 bits
+    uint8_t  color16to8(uint16_t color565);
+
+    // Convert 16 bit colour to/from 24 bit, R+G+B concatenated into LS 24 bits
+    uint32_t color16to24(uint16_t color565);
+    uint32_t color24to16(uint32_t color888);
+
+    // Alpha blend 2 colours, see generic "alphaBlend_Test" example
+    // alpha =   0 = 100% background colour
+    // alpha = 255 = 100% foreground colour
+    uint16_t alphaBlend(uint8_t alpha, uint16_t fgc, uint16_t bgc);
+    // 16 bit colour alphaBlend with alpha dither (dither reduces colour banding)
+    uint16_t alphaBlend(uint8_t alpha, uint16_t fgc, uint16_t bgc, uint8_t dither);
+    // 24 bit colour alphaBlend with optional alpha dither
+    uint32_t alphaBlend24(uint8_t alpha, uint32_t fgc, uint32_t bgc, uint8_t dither = 0);
+
+
+    // DMA support functions - these are currently just for SPI writes whe using the STM32 processors
+    // Bear in mind DMA will only be of benefit in particular circumstances and can be tricky
+    // to manage by noobs. The functions have however been designed to be noob friendly and
+    // avoid a few DMA behaviour "gotchas".
+    //
+    // At best you will get a 2x TFT rendering performance improvement when using DMA because
+    // this library handles the SPI bus so efficiently during normal (non DMA) transfers. The best
+    // performance improvement scenario is the DMA transfer time is exactly the same as the time it
+    // takes for the processor to prepare the next image buffer and initiate another DMA transfer.
+    //
+    // DMA transfer to the TFT is done while the processor moves on to handle other tasks. Bear
+    // this in mind and watch out for "gotchas" like the image buffer going out of scope as the
+    // processor leaves a function or its content being changed while the DMA engine is reading it.
+    //
+    // The compiler MAY change the implied scope of a buffer which has been set aside by creating
+    // and an array. For example a buffer defined before a "for-next" loop may get de-allocated when
+    // the loop ends. To avoid this use, for example, malloc() and free() to take control of when
+    // the buffer space is available and ensure it is not released until DMA is complete.
+    //
+    // Clearly you should not modify a buffer that is being DMA'ed to the TFT until the DMA is over.
+    // Use the dmaBusy() function to check this.  Use tft.startWrite() before invoking DMA so the
+    // TFT chip select stays low. If you use tft.endWrite() before DMA is complete then the endWrite
+    // function will wait for the DMA to complete, so this may defeat any DMA performance benefit.
+    //
+
+    bool     initDMA(void);     // Initialise the DMA engine and attach to SPI bus - typically used in setup()
+    void     deInitDMA(void);   // De-initialise the DMA engine and detach from SPI bus - typically not used
+
+    // Push an image to the TFT using DMA, buffer is optional and grabs (double buffers) a copy of the image
+    // Use the buffer if the image data will get over-written or destroyed while DMA is in progress
+    // If swapping colour bytes is defined, and the double buffer option is NOT used then the bytes
+    // in the original data image will be swapped by the function before DMA is initiated.
+    // The function will wait for the last DMA to complete if it is called while a previous DMA is still
+    // in progress, this simplifies the sketch and helps avoid "gotchas".
+    void     pushImageDMA(int32_t x, int32_t y, int32_t w, int32_t h, uint16_t *data, uint16_t *buffer = nullptr);
+
+    // Push a block of pixels into a window set up using setAddrWindow()
+    void     pushPixelsDMA(uint16_t *image, uint32_t len);
+
+    // Check if the DMA is complete - use while(tft.dmaBusy); for a blocking wait
+    bool     dmaBusy(void); // returns true if DMA is still in progress
+    void     dmaWait(void); // wait until DMA is complete
+
+    bool     DMA_Enabled = false;   // Flag for DMA enabled state
+    uint8_t  spiBusyCheck = 0;      // Number of ESP32 transfer buffers to check
+
+    // Bare metal functions
+    void     startWrite(void);                         // Begin SPI transaction
+    void     writeColor(uint16_t color, uint32_t len); // Deprecated, use pushBlock()
+    void     endWrite(void);                           // End SPI transaction
+
+    // Set/get an arbitrary library configuration attribute or option
+    //       Use to switch ON/OFF capabilities such as UTF8 decoding - each attribute has a unique ID
+    //       id = 0: reserved - may be used in fuuture to reset all attributes to a default state
+    //       id = 1: Turn on (a=true) or off (a=false) GLCD cp437 font character error correction
+    //       id = 2: Turn on (a=true) or off (a=false) UTF8 decoding
+    //       id = 3: Enable or disable use of ESP32 PSRAM (if available)
+#define CP437_SWITCH 1
+#define UTF8_SWITCH  2
+#define PSRAM_ENABLE 3
+    void     setAttribute(uint8_t id = 0, uint8_t a = 0); // Set attribute value
+    uint8_t  getAttribute(uint8_t id = 0);                // Get attribute value
+
+    // Used for diagnostic sketch to see library setup adopted by compiler, see Section 7 above
+    void     getSetup(setup_t &tft_settings); // Sketch provides the instance to populate
+
+    // Global variables
+    static   SPIClass &getSPIinstance(void); // Get SPI class handle
+
+    int32_t  cursor_x, cursor_y, padX;       // Text cursor x,y and padding setting
+    uint32_t textcolor, textbgcolor;         // Text foreground and background colours
+
+    uint32_t bitmap_fg, bitmap_bg;           // Bitmap foreground (bit=1) and background (bit=0) colours
+
+    uint8_t  textfont,  // Current selected font number
+             textsize,  // Current font size multiplier
+             textdatum, // Text reference datum
+             rotation;  // Display rotation (0-3)
+
+    int16_t  _xpivot;   // TFT x pivot point coordinate for rotated Sprites
+    int16_t  _ypivot;   // TFT x pivot point coordinate for rotated Sprites
+
+    uint8_t  decoderState = 0;   // UTF8 decoder state        - not for user access
+    uint16_t decoderBuffer;      // Unicode code-point buffer - not for user access

src/twali_screen.h

@@ -5,7 +5,11 @@
 extern "C" {
 #endif
 
-struct twali_screen
+struct twali_screen;
+
+struct twali_screen *
+twali_screen_sub(struct twali_screen s, x int, y int, w int, h int);
+
 
 #ifdef __cplusplus
 }

src/twali_system.h

@@ -7,6 +7,7 @@ extern "C" {
 
 struct twali_system;
 struct twali_power;
+struct twali_task;
 
 struct twali_power * twali_system_power(struct twali_system * s);
 
@@ -17,6 +18,28 @@ void twali_system_update(struct twali_system * s);
 
 void twali_system_wake_up(struct twali_system *s);
 
+struct upubsub_listener * upubsub_subscribe_listener(struct upubsub * u, struct upubsub_listener l);
+struct upubsub_listener* upubsub_subscribe(struct upubsub * u, const char * topic, void * data, upubsub_listen_func * listen);
+int upubsub_publish_message(struct upubsub u, struct upubsub_message m);
+int upubsub_publish_data(struct upubsub u, const char * topic, void * data, size_t size);
+int upubsub_publish_str(struct upubsub u, const char * topic, char * str);
+int upubsub_unsubscribe_listener(struct upubsub * u, struct upubsub_listener * l);
+
+void twali_system_publish_data(struct twali_system *s, const char * topic, void * mesg, size_t size);
+void twali_system_publish_str(struct twali_system *s, const char * topic, char * str);
+
+struct upubsub_listener * twali_system_subscribe_listener(struct twali_system *s, 
+const char * topic, void * data, upubsub_listen_func * listen);
+struct upubsub_listener * twali_system_subscribe_task(struct twali_system *s, 
+const char * topic, void * data, struct twali_task * task);
+int twali_system_unsubscribe_listener(struct twali_system *s, 
+const char * topic, struct upubsub_listener * listener);
+struct upubsub_listener * twali_system_unsubscribe_task(struct twali_system *s, 
+const char * topic,  struct twali_task * task);
+
+
+
+
 #ifdef __cplusplus
 }
 #endif

src/twali_system_internal.h

@@ -14,6 +14,7 @@
 #include <TTGO.h>
 #endif
 
+#include "upubsub.h"
 #include "twali_system.h"
 #include "twali_power.h"
 #include "twali_screen.h"
@@ -27,6 +28,7 @@ struct twali_system {
     TFT_eSPI *tft;
     struct twali_power * power;
     struct twali_screen * screen;
+    struct upubsub bus;
 };
 
 #ifdef __cplusplus

src/twali_task.c

@@ -7,6 +7,9 @@
 #include "freertos/queue.h"
 #include "freertos/task.h"
 
+#define UPUBSUB_IMPLEMENTATION
+#include "upubsub.h"
+
 struct twali_task {
     struct twali_system * system;
     struct twali_task   * parent;

src/upubsub.h

@@ -14,6 +14,10 @@
 #define UPUBSUB_TOPIC_AMOUNT 32
 #endif /* UPUBSUB_TOPIC_AMOUNT */
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include <stdlib.h>
 #include <stdint.h>
 #include <string.h>
@@ -40,18 +44,11 @@ struct upubsub_listener {
 	upubsub_listen_func * listen;
 };
 
-struct upubsub_topic {
-	const char * topic;
-	size_t size;
-	struct upubsub_listener * first;
-};
-
 struct upubsub {
 	size_t size;
 	unsigned int last_id;
 	struct upubsub_listener listeners[UPUBSUB_LISTENER_AMOUNT];
 	size_t topics_size;
-	struct upubsub_topic topics[UPUBSUB_TOPIC_AMOUNT];
 };
 
 
@@ -65,6 +62,12 @@ int upubsub_publish_data(struct upubsub u, const char * topic, void * data, size
 int upubsub_publish_str(struct upubsub u, const char * topic, char * str);
 int upubsub_unsubscribe_listener(struct upubsub * u, struct upubsub_listener * l);
 
+
+#ifdef __cplusplus
+}
+#endif
+
+
 #endif /* UPUBSUB_H_INCLUDED */
 
 #ifdef UPUBSUB_TEST
@@ -73,45 +76,51 @@ int upubsub_unsubscribe_listener(struct upubsub * u, struct upubsub_listener * l
 
 #ifdef UPUBSUB_IMPLEMENTATION
 
-int bsearch_low_index(const void *key, const void *base, size_t nel, size_t width, int (*cmp)(const void *, const void *)) {
-	void *try;
+#ifdef __cplusplus
+#error This is not C++. Please compile this with a C compiler.
+#endif
+
+/** Result of searching with brange_search. If high < low, nothing was found. */
+struct bsearch_range_result {
+    int low;
+    int high;
+};
+
+struct bsearch_range_result bsearch_range (const void *key, const void *base, size_t nel, size_t width, int (*cmp)(const void *, const void *)) {
+    struct bsearch_range_result res;
+	void *here;
 	int sign;
-    int found;
-    int low = 0;
-    int high = nel;    
-	while (low <= high) {
-        int mid = low + (high - low) / 2;
-        try = (char*)base + (mid*width);
-		sign = cmp(key, try);
+    int found;    
+    int botm = 0;
+    int top = nel;    
+    res.low = 0;
+    res.high = nel;
+	while (res.low <= top) {
+        int mid = res.low + (top - res.low) / 2;
+        here = (char*)base + (mid*width);
+		sign = cmp(key, here);
 		if (sign <= 0) {
-			high = mid - 1;
+			top = mid - 1;
 		} else {
-			low = mid + 1;
+			res.low = mid + 1;
 		}
 	}
-	return low;
-}
-
-int bsearch_high_index(const void *key, const void *base, size_t nel, size_t width, int (*cmp)(const void *, const void *)) {
-	void *try;
-	int sign;
-    int found;
-    int low = 0;
-    int high = nel;    
-	while (low <= high) {
-        int mid = low + (high - low) / 2;
-        try = (char*)base + (mid*width);
-		sign = cmp(key, try);
+    botm = res.low ;
+	while (botm <= res.high) {
+        int mid = botm + (res.high - botm) / 2;
+        here = (char*)base + (mid*width);
+		sign = cmp(key, here);
 		if (sign < 0) {
-			high = mid - 1;
+			res.high = mid - 1;
 		} else {
-			low = mid + 1;
+			botm = mid + 1;
 		}
 	}
-	return high;
+	return res;
 }
 
 
+
 int upubsub_listener_cmp_topic_id(const void * v1, const void * v2) {
 	int res;
 	struct upubsub_listener *l1, *l2;
@@ -125,36 +134,23 @@ int upubsub_listener_cmp_topic_id(const void * v1, const void * v2) {
      	/* Otherwise compare topics. */
 	res = strcmp(l1->topic, l2->topic);
 	/* If the same sort by id to try to keep the sort stable. */
-	if (!res) {
+	 if (!res) {
 		return l1->id - l2->id;
 	}
 	return res;
 }
 
-int upubsub_topic_cmp_topic(const void * v1, const void * v2) {
-	struct upubsub_topic *l1, *l2;
-	l1 = (struct upubsub_topic *)(v1);
-	l2 = (struct upubsub_topic *)(v2);
-
-	/* Sort with null topic as last, as these ones are unused. */
-	if ((!l1->topic) && (l2->topic)) { return 1;  }
-	if ((!l2->topic) && (l1->topic)) { return -1; }
-	if ((!l2->topic) && (!l1->topic)) { return 0; }
-     	/* Otherwise compare topics. */
-	return strcmp(l1->topic, l2->topic);
-}
-
 int upubsub_listener_cmp_topic(const void * v1, const void * v2) {
-	struct upubsub_listener *l1, *l2;
-	l1 = (struct upubsub_listener *)(v1);
-	l2 = (struct upubsub_listener *)(v2);
-
-	/* Sort with null topic as last, as these ones are unused. */
-	if ((!l1->topic) && (l2->topic)) { return 1;  }
-	if ((!l2->topic) && (l1->topic)) { return -1; }
-	if ((!l2->topic) && (!l1->topic)) { return 0; }
-     	/* Otherwise compare topics. */
-	return strcmp(l1->topic, l2->topic);
+       struct upubsub_listener *l1, *l2;
+       l1 = (struct upubsub_listener *)(v1);
+       l2 = (struct upubsub_listener *)(v2);
+
+       /* Sort with null topic as last, as these ones are unused. */
+       if ((!l1->topic) && (l2->topic)) { return 1;  }
+       if ((!l2->topic) && (l1->topic)) { return -1; }
+       if ((!l2->topic) && (!l1->topic)) { return 0; }
+       /* Otherwise compare topics. */
+       return strcmp(l1->topic, l2->topic);
 }
 
 int upubsub_listener_cmp_id(const void * v1, const void * v2) {
@@ -199,16 +195,16 @@ int upubsub_publish_message(struct upubsub u, struct upubsub_message m) {
 	size_t start, index;
 	struct upubsub_listener * found = NULL;
 	struct upubsub_listener key;
-    int low, high;
+    struct bsearch_range_result ran;
 
 	key.topic = m.topic;
 	key.id = 1;    
-	low = bsearch_low_index(&key, u.listeners, u.size, sizeof(struct upubsub_listener), upubsub_listener_cmp_topic);
-    high = bsearch_high_index(&key, u.listeners, u.size, sizeof(struct upubsub_listener), upubsub_listener_cmp_topic);
-	if (high < low) {
+	ran = bsearch_range(&key, u.listeners, u.size, sizeof(struct upubsub_listener), upubsub_listener_cmp_topic);    
+	if (ran.high < ran.low) { /* Not found, bail out */
 		return sent;
-	}	
-	for (index = low; index <= high; index ++) {
+	}
+    /* Broadcast to all in range. */
+	for (index = ran.low; index <= ran.high; index ++) {
 		found = u.listeners + index;
 		found->listen(found, m);
 		sent ++;
@@ -254,8 +250,8 @@ int upubsub_unsubscribe(struct upubsub_listener * l)  {
 	return upubsub_unsubscribe_listener(l->upubsub, l);
 }
 
-#endif /* UPUBSUB_IMPLEMENTATION */
 
+#endif /* UPUBSUB_IMPLEMENTATION */
 
 
 #ifdef UPUBSUB_TEST
@@ -279,25 +275,24 @@ int main(void) {
     int arr[] = { 7, 8 , 8, 9, 9, 9, 10, 11, 12, 20, 20, 32 };
     int low, high;
     int key = 9;
-    low = bsearch_low_index(&key, &arr, 12, sizeof(int), compare_int);
-    high = bsearch_high_index(&key, &arr, 12, sizeof(int), compare_int);
-    printf("Low, high: %d -> %d...\n", low, high);
-    if ((low > 0) && (low < 12)) {
-        printf("Low: %d\n", arr[low]);
+    struct bsearch_range_result ran;
+    ran = bsearch_range(&key, arr, 12, sizeof(int), compare_int);
+    printf("Low, high: %d -> %d...\n ", ran.low, ran.high);
+    if ((ran.low >= 0) && (ran.low < 12)) {
+        printf("Low: %d\n", arr[ran.low]);
     }
-    if ((high > 0) && (high < 12)) {
-        printf("High: %d\n", arr[high]);
+    if ((ran.high >= 0) && (ran.high < 12)) {
+        printf("High: %d\n", arr[ran.high]);
     }
     
     for (key = 0; key < 40; key ++) {     
-        low = bsearch_low_index(&key, &arr, 12, sizeof(int), compare_int);
-        high = bsearch_high_index(&key, &arr, 12, sizeof(int), compare_int);
-        printf("Key: %d; Low, high: %d -> %d:", key, low, high);
-        if ((low >= 0) && (low < 12)) {
+        ran = bsearch_range(&key, arr, 12, sizeof(int), compare_int);
+        printf("Key: %d; Low, high: %d -> %d:", key, ran.low, ran.high);
+        if ((ran.low >= 0) && (ran.low < 12)) {
             printf(" Low: %d", arr[low]);
         }
-        if ((high >= 0) && (high < 12)) {
-            printf(" High: %d", arr[high]);
+        if ((ran.high >= 0) && (ran.high < 12)) {
+            printf(" High: %d", arr[ran.high]);
         }
         printf("\n");
     }
@@ -309,6 +304,9 @@ int main(void) {
 	l[1] = upubsub_subscribe(&pusu, "POWER" , "APP1", listen_print_str);
 	l[2] = upubsub_subscribe(&pusu, "SYSTEM", "APP2", listen_print_str);
 	l[3] = upubsub_subscribe(&pusu, "SYSTEM", "WIND", listen_print_str);
+    l[2] = upubsub_subscribe(&pusu, "SYSTEM", "APP3", listen_print_str);
+	l[3] = upubsub_subscribe(&pusu, "POWER", "APP4", listen_print_str);
+
 
 	printf("Publishing...\n");
 	upubsub_publish_str(pusu, "POWER", "DOWN");