Q:

Need to write algorithm in state-machine style, but it becomes very hard to read

I work on embedded device's firmware (write in C), I need to take a screenshot from the display and save it as a bmp file.  Currently I work on the module that generates bmp file data.  The easiest way to do that is to write some function that takes the following arguments:
(for simplicity, only images with indexed colors are supported in my example)

color_depth
image size (width, height)
pointer to function to get palette color for color_index (i)
pointer to function to get color_index of the pixel with given coords (x, y)
pointer to function to write image data

And then user of this function should call it like that:
/*
 * Assume we have the following functions:
 *    int_least32_t palette_color_get  (int color_index);
 *    int           pix_color_idx_get  (int x, int y);
 *    void          data_write         (const char *p_data, size_t len);
 */

bmp_file_generate(
      1,         //-- color_depth
      x, y,      //-- size
      palette_color_get,
      pic_color_idx_get,
      data_write
      );

And that's it: this functions does all the job, and returns only when job is done (i.e. bmp file generated and "written" by given user callback function data_write(). 
BUT, I need to make bmp_writer module to be usable in cooperative RTOS, and data_write() might be a function that actually transmits data via some protocol (say, UART) to another device), so, this function needs to be called only from Task context.  This approach doesn't work then, I need to make it in OO-style, and its usage should look like this:
/*
 * create instance of bmp_writer with needed params
 * (we don't need "data_write" pointer anymore)
 */
T_BmpWriter *p_bmp_writer = new_bmp_writer(
      1,         //-- color_depth
      x, y,      //-- size
      palette_color_get,
      pic_color_idx_get
      );

/*
 * Now, byte-by-byte get all the data! 
 */
while (bmp_writer__data_available(p_bmp_writer) > 0){
   char cur_char = bmp_writer__get_next_char(p_bmp_writer);
   //-- do something useful with current byte (i.e. cur_char). 
   //   maybe transmit to another device, or save to flash, or anything. 
}

/*
 * Done!  Free memory now. 
 */ 
delete_bmp_writer(p_bmp_writer);

As you see, user can call bmp_writer__get_next_char(p_bmp_writer) when he need that, and handle received data as he wants. 
Actually I already implemented this, but, with that approach, all the algorithm becomes turned inside out, and this code is extremely non-readable. 
I'll show you a part of old code that generates palette data (from the function that does all the job, and returns only when job is done), and appropriate part of new code (in state-machine style). 
Old code:
void bmp_file_generate(/*....args....*/)
{
   //-- ... write headers

   //-- write palette (if needed)
   if (palette_colors_cnt > 0){
      size_t i;
      int_least32_t cur_color;
      for (i = 0; i < palette_colors_cnt; i++){
         cur_color = callback_palette_color_get(i);
         callback_data_write((const char *)&cur_color, sizeof(cur_color));
      }
   }

   //-- ...... write image data ..........
}

As you see, very short and easy-readable code. 
Now, new code.  
It looks like state-machine, because it's actually splitted by stages (HEADER_WRITE, PALETTE_WRITE, IMG_DATA_WRITE), each stage has its own context.  In the old code, context was saved in local variables, but now we need to make the structure and allocate it from heap. 
So:
/*
 * Palette stage context
 */
typedef struct {
   size_t         i;
   size_t         cur_color_idx;
   int_least32_t  cur_color;
} T_StageContext_Palette;

/*
 * Function that switches stage. 
 * T_BmpWriter is an object context, and pointer *me is analogue of "this" in OO-languages. 
 * bool_start is 1 if stage is just started, and 0 if it is finished. 
 */
static void _stage_start_end(T_BmpWriter *me, U08 bool_start)
{
   switch (me->stage){
      //-- ...........other stages.........

      case BMP_WR_STAGE__PALETTE:
         if (bool_start){
            //-- palette stage is just started.  Allocate stage context and initialize it. 
            me->p_stage_context = malloc(sizeof(T_StageContext_Palette));
            memset(me->p_stage_context, 0x00, sizeof(T_StageContext_Palette));

            //-- we need to get first color, so, set index of byte in cur_color to maximum
            ((T_StageContext_Palette *)me->p_stage_context)->i = sizeof(int_least32_t);
         } else {
            free(me->p_stage_context);
            me->p_stage_context = NULL;
         }
         break;

      //-- ...........other stages.........

   }
}

/*
 * Function that turns to the next stage
 */
static void _next_stage(T_BmpWriter *me)
{
   _stage_start_end(me, 0);
   me->stage++;
   _stage_start_end(me, 1);
}

/*
 * Function that actually does the job and returns next byte
 */
U08 bmp_writer__get_next_char(T_BmpWriter *me)
{
   U08 ret = 0;        //-- resulting byte to return
   U08 bool_ready = 0; //-- flag if byte is ready

   while (! bool_ready){
      switch (me->stage){

         //-- ...........other stages.........

         case BMP_WR_STAGE__PALETTE:
            {
               T_StageContext_Palette *p_stage_context =
                  (T_StageContext_Palette *)me->p_stage_context;

               if (p_stage_context->i < sizeof(int_least32_t)){
                  //-- return byte of cur_color
                  ret = *( (U08 *)&p_stage_context->cur_color + p_stage_context->i );
                  p_stage_context->i++;
                  bool_ready = 1;
               } else {
                  //-- need to get next color (or even go to next stage)

                  if (p_stage_context->cur_color_idx < me->bmp_details.palette_colors_cnt){
                     //-- next color
                     p_stage_context->cur_color = me->callback.p_palette_color_get(
                           me->callback.user_data,
                           p_stage_context->cur_color_idx
                           );
                     p_stage_context->cur_color_idx++;
                     p_stage_context->i = 0;
                  } else {
                     //-- next stage! 
                     _next_stage(me);
                  }
               }

            }
            break;

         //-- ...........other stages.........

      }
   }

   return ret;

}

So huge code, and it's so hard to understand it! 
But I really have no idea how to make it in some different way, to be able to get information byte-by-byte. 
Does anyone know how to achieve this, and keep code readability? 
Any help is appreciated. 

A:

You can try protothread, which is useful to transform a state-machine based program into thread-style program.  I'm not 100% sure that it can solve your problem elegantly, you can give it a try.  The paper is a good starting point: Protothreads: simplifying event-driven programming of memory-constrained embedded systems
Here is its source code: http://code.google.com/p/protothread/
By the way, protothread is also used in the Contiki embedded OS, for implementing process in Contiki. 