/***************************************************************************
 * Copyright (c) 2005, Research Group Embedded Interaction                 *
 * Paul Holleis <paul@hcilab.org>                                          *
 * Matthias Kranz <matthias@hcilab.org>                                    *
 * Albrecht Schmidt <albrecht@hcilab.org>                                  *
 * All rights reserved.                                                    *
 *                                                                         *
 * Redistribution and use in source and binary forms, with or without      *
 * modification, are permitted provided that the following conditions      *
 * are met:                                                                *
 *                                                                         *
 *    * Redistributions of source code must retain the above copyright     *
 *      notice, this list of conditions and the following disclaimer.      *
 *    * Redistributions in binary form must reproduce the above copyright  *
 *      notice, this list of conditions and the following disclaimer in    *
 *      the documentation and/or other materials provided with the         *
 *      distribution.                                                      *
 *    * Neither the name of the <ORGANIZATION> nor the names of its        *
 *      contributors may be used to endorse or promote products derived    *
 *      from this software without specific prior written permission.      *
 *                                                                         *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS     *
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT       *
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR   *
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT    *
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,   *
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT        *
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   *
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON       *
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR      *
 * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF      *
 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH    *
 * DAMAGE.                                                                 *
 **************************************************************************/
//#if (!(PIN_DISPLAY_1_I2C_SDA && PIN_DISPLAY_1_I2C_SCL && PIN_DISPLAY_2_I2C_SDA && PIN_DISPLAY_2_I2C_SCL && PIN_DISPLAY_3_I2C_SDA && PIN_DISPLAY_3_I2C_SCL && PIN_DISPLAY_4_I2C_SDA && PIN_DISPLAY_4_I2C_SCL && PIN_DISPLAY_5_I2C_SDA && PIN_DISPLAY_5_I2C_SCL))
//#error Please define I2C pins (e.g., use bt96040-addonboard.h)
//#endif

#ifndef __BT96040_C_
#define __BT96040_C_

#ifndef __BT96040_DS_H_
#ifdef __LINUX__
#include "actuators/bt96040_ds.h"
#else
#include "actuators\bt96040_ds.h"
#endif
#endif

//#use i2c(master, sda=PIN_DISPLAY_1_I2C_SDA, scl=PIN_DISPLAY_1_I2C_SCL)



/* ---------------------------------------------------------------------------- */



// debug template

//PackInACL((unsigned int)r, ACL_TYPE_SGY_HI, ACL_TYPE_SGY_LO);
//PackInACL((unsigned int)c, ACL_TYPE_SGX_HI, ACL_TYPE_SGX_LO);
//while(ACLSendingBusy()){;}
//ACLSendPacket( 20 );

// char text[17];
//sprintf(temptext, "text %i %s", rotation, text);
//bt_write_string_display(0, BT_ROW_1,BT_CHAR_1, temptext);






/******************************************************************************/
/* HELPER FUNCTIONS                                                           */
/******************************************************************************/
/**
 * Convert row / col into array index.
 * \param row in [0, MAX_ROW]
 * \param col in [0, MAX_COL]
 */
unsigned long getIndex(unsigned int row, unsigned int col) {
  unsigned long r, c;
  r = row;
  c = col;
  return (r / 8)*(MAX_COL +1) + c;
  ///return (row / 8)*(MAX_COL +1) + col;
      }
/**
 * Convert row / col into array index.
 * \param row in [0, MAX_ROW / 8]
 * \param col in [0, MAX_COL]
 */
unsigned long getByteIndex(unsigned int row, unsigned int col) {
  unsigned long r, c;
  r = row;
  c = col;
  return r*(MAX_COL +1) + c;
  ///return row*(MAX_COL +1) + col;
      }

/**
 * Converts given coordinates row/col (on an area that has maxrow/maxcol
 * as maximum row / col numbers) to rotated coordinates new_row/new_col. 
 * deg must be DEGx
 * memory for new_row and new_col must have been allocated
 * \return -1 if new coordinates are not within [0,rows]/[0,cols],
 *          0 if no error occurred
 */
signed int convert_coords(int deg, unsigned int row, unsigned int col,
         int *new_row, int *new_col, unsigned int maxrow, unsigned int maxcol) {

   if (deg == DEG0) {
      *new_row = row;
      *new_col = col;
   } else if (deg == DEG90) {
      *new_row = maxrow - col;
      *new_col = row;
   } else if (deg == DEG180) {
      *new_row = maxrow - row;
      *new_col = maxcol - col;
   } else if (deg == DEG270) {
      *new_row = col;
      *new_col = maxcol - row;
   }

   if (insideSignedMax(*new_row, *new_col, maxrow, maxcol))
      return 0;
   else
      return -1;
}

/**
 * Needs a byte[6]. Writes a bit representation of the given character
 * into that byte array. Used table1 and table2.
 * \return 0 if no error occurred
 */
signed int get_letter(char ch, byte *b) {
   int tab_index;
   unsigned int i;

   b[0] = b[1] = b[2] = b[3] = b[4] = b[5] = 0;

   if (ch < 0x20 || ch > 0x7f) return -1;

   for (i = 0; i < 5; i++) {
      if (ch < 0x50) {
         tab_index = (((ch&0xff)-0x20)*5);
         b[i] = table1[(tab_index+i)];
      } 
      else //if (ch > 0x4f)
      {
            tab_index=(((ch&0xff)-0x50)*5);
            b[i] = table2[(tab_index+i)];
      }
   }   
   // b[5] = 0;
   return 0;
}






/******************************************************************************/
/* METHODS USING FLASH MEMORY FOR DISPLAY BUFFERS                             */
/******************************************************************************/
/**
 * Writes bufferdisplay to the appropriate flash memory page
 * \param dispnr the number of the display you want to save
 */
signed int bufferToFlash(unsigned int dispnr) {
   int32 mempos;
   unsigned long lengthwritten, towrite, i;
   const unsigned long len = DISP_MEM;
   unsigned long count;
   unsigned byte writebuffer[FLASH_PAGE_SIZE];

   if (dispnr > 5) return -2;

   //   // nothing to do if correct display already buffered
   //   if (bufferDisplayNumber == dispnr) return 0;

   mempos = DISPLAY_0_BUFFER_FLASH;
   if (dispnr == 1) mempos = DISPLAY_1_BUFFER_FLASH;
   else if (dispnr == 2) mempos = DISPLAY_2_BUFFER_FLASH;
   else if (dispnr == 3) mempos = DISPLAY_3_BUFFER_FLASH;
   else if (dispnr == 4) mempos = DISPLAY_4_BUFFER_FLASH;
   else if (dispnr == 5) mempos = DISPLAY_5_BUFFER_FLASH;

   count = 0;
   lengthwritten = 0;
   while (lengthwritten < len) {
      towrite = FLASH_PAGE_SIZE;
      if (len - lengthwritten < FLASH_PAGE_SIZE) towrite = len - lengthwritten;
      // write part of the display data into flash
      // TODO check whether can directly write to writebuffer
      // copy to writebuffer
      for (i = 0; i < towrite; i++) {
         writebuffer[i] = displaybuffer[count];
         count++;
      }

      while (!FlashWriteSequence(mempos, writebuffer, towrite)) {;}

      lengthwritten += towrite;
      mempos += towrite;
   }

   //   bufferDisplayNumber = dispnr;

   return 0;
}


/**
 * Ensures that bufferdisplay contains the data from given display
 * \param dispnr the number of the display you want to access
 */
signed int flashToBuffer(unsigned int dispnr) {
   int32 mempos;
   unsigned long lengthread, toread, i;
   const unsigned long len = DISP_MEM;
   unsigned long count;
   unsigned byte readbuffer[FLASH_PAGE_SIZE];

   if (dispnr > 5) return -2;

   // nothing to do if correct display already buffered
   if (bufferDisplayNumber == dispnr) return 0;
   
   // write back the buffer into flash memory
   bufferToFlash(bufferDisplayNumber);   

   mempos = DISPLAY_0_BUFFER_FLASH;
   if (dispnr == 1) mempos = DISPLAY_1_BUFFER_FLASH;
   else if (dispnr == 2) mempos = DISPLAY_2_BUFFER_FLASH;
   else if (dispnr == 3) mempos = DISPLAY_3_BUFFER_FLASH;
   else if (dispnr == 4) mempos = DISPLAY_4_BUFFER_FLASH;
   else if (dispnr == 5) mempos = DISPLAY_5_BUFFER_FLASH;

   count = 0;
   lengthread = 0;
   while (lengthread < len) {
      toread = FLASH_PAGE_SIZE;
      if (len - lengthread < FLASH_PAGE_SIZE) toread = len - lengthread;
      // read part of the display data from flash
      // TODO check whether can directly read into displaybuffer
      while (!FlashReadSequence(mempos, readbuffer, toread)) {;}

      // copy to displaybuffer
      for (i = 0; i < toread; i++) {
         displaybuffer[count] = readbuffer[i];
         count++;
      }

      lengthread += toread;
      mempos += toread;
   }

   bufferDisplayNumber = dispnr;

   return 0;
}







/******************************************************************************/
/* BUFFER ACCESS                                                              */
/******************************************************************************/

// ---------------------- STANDARD DISPLAY ------------------------
#ifdef FAST_DISPLAY_0
/**
 * row in [0,ROW_MAX] 
 */
signed int dOrBit(signed int row, signed int col, int1 thebit) {
   unsigned byte b;

   if (!insideSigned(row, col)) return -1;

   ///b = display[row / 8][col];
      b = display0[getIndex(row, col)];
      if (thebit || bit_test(b, row % 8))
         bit_set(b, row % 8);

      ///   display[row / 8][col] = b;
         display0[getIndex(row, col)] = b;

         return 0;
}

/**
 * row in [0,ROW_MAX], col in [0,COL_MAX]
 */
signed int dSetBit(signed int row, signed int col, int1 thebit) {
   //    return setBitPtr(display, row, col, thebit);
   unsigned byte b;

   if (!insideSigned(row, col)) return -1;

   ///   b = display[row / 8][col];
      b = display0[getIndex(row, col)];
      if (thebit == 1)
         bit_set(b, row % 8);
      else
         bit_clear(b, row % 8);
      ///   display[row / 8][col] = b;
         display0[getIndex(row, col)] = b;

         return 0;
}

/**
 * row in [0,ROW_MAX], col in [0,COL_MAX]
 */
int1 dGetBit(signed int row, signed int col) {
   //    return getBitPtr(display, row, col);
   unsigned byte b;

   if (!insideSigned(row, col)) return 0;

   ///b = display[row / 8][col];
      b = display0[getIndex(row, col)];
      return bit_test(b, row % 8);
}

/**
 * sets byte in buffer
 * row = [0, MAX_ROW / 8], col = [0, MAX_COL]
 */
signed int dSetByte(signed int row, signed int col, byte thebyte) {
   //    return setBytePtr(display, row, col, thebyte);
   if (!insideSigned(row * 8, col)) return -1;

   ///   display[row][col] = thebyte;
      display0[getByteIndex(row, col)] = thebyte;
      return 0;
}

/**
 * gets byte from buffer
 * row = [0, MAX_ROW / 8], col = [0, MAX_COL]
 */
signed byte dGetByte(signed int row, signed int col) {
   //    return getBytePtr(display, row, col);
   if (!insideSigned(row * 8, col)) return -1;

   ///   return display[row][col];
      return display0[getByteIndex(row, col)];
}
#endif



// ---------------------- MULTIPLE DISPLAYS ------------------------
/**
 * row in [0,ROW_MAX] 
 */
signed int dsOrBit(unsigned int dispnr, signed int row, signed int col, int1 thebit) {
   unsigned byte b;

#ifdef FAST_DISPLAY_0
   if (dispnr == 0) return dOrBit(row, col, thebit);
#endif
   if (!insideSigned(row, col)) return -1;

   flashToBuffer(dispnr);

   b = displaybuffer[getIndex(row, col)];
   if (thebit || bit_test(b, row % 8))
      bit_set(b, row % 8);

   displaybuffer[getIndex(row, col)] = b;
   return 0;
}

/**
 * row in [0,ROW_MAX], col in [0,COL_MAX]
 */
signed int dsSetBit(unsigned int dispnr, signed int row, signed int col, int1 thebit) {
   //    return setBitPtr(display, row, col, thebit);
   unsigned byte b;

#ifdef FAST_DISPLAY_0
   if (!insideSigned(row, col)) return -1;
   if (dispnr == 0) return dSetBit(row, col, thebit);
#endif
   //v   if (!inside(row, col)) return -1;
   //v+
   if (!insideSigned(row, col)) return -1;

   flashToBuffer(dispnr);

   b = displaybuffer[getIndex(row, col)];

   if (thebit == 1)
      bit_set(b, row % 8);
   else
      bit_clear(b, row % 8);

   displaybuffer[getIndex(row, col)] = b;
   return 0;
}

/**
 * row in [0,ROW_MAX], col in [0,COL_MAX]
 */
int1 dsGetBit(unsigned int dispnr, signed int row, signed int col) {
   //    return getBitPtr(display, row, col);
   unsigned byte b;

#ifdef FAST_DISPLAY_0
   if (dispnr == 0) return dGetBit(row, col);
#endif
   if (!insideSigned(row, col)) return 0;

   flashToBuffer(dispnr);

   b = displaybuffer[getIndex(row, col)];
   return bit_test(b, row % 8);
}

/**
 * sets byte in buffer
 * row = [0, MAX_ROW / 8], col = [0, MAX_COL]
 */
signed int dsSetByte(unsigned int dispnr, signed int row, signed int col, byte thebyte) {
   //    return setBytePtr(display, row, col, thebyte);
#ifdef FAST_DISPLAY_0
   if (dispnr == 0) return dSetByte(row, col, thebyte);
#endif
   if (!insideSigned(row * 8, col)) return -1;

   flashToBuffer(dispnr);

   displaybuffer[getByteIndex(row, col)] = thebyte;
   return 0;
}

/**
 * gets byte from buffer
 * row = [0, MAX_ROW / 8], col = [0, MAX_COL]
 */
signed byte dsGetByte(unsigned int dispnr, signed int row, signed int col) {
   //    return getBytePtr(display, row, col);
#ifdef FAST_DISPLAY_0
   if (dispnr == 0) return dGetByte(row, col);
#endif
   if (!insideSigned(row * 8, col)) return -1;

   flashToBuffer(dispnr);

   return displaybuffer[getByteIndex(row, col)];
}









/******************************************************************************/
/* METHODS WRITING FROM THE DISPLAY BUFFER TO THE DISPLAY                     */
/******************************************************************************/



/**
 * writes the byte at the specified position (row,col) from buffer to display
 * row = [0, MAX_ROW], col = [0, MAX_COL]
 */
signed int displayByteUpdate(unsigned int dispnr, signed int row, signed int col) {
   if (! (insideSigned(row, col))) return -1;
   //if (!inside(row, col)) return -1;
   if (dispnr > 5) return -2;

   // this code is partly taken from the old write_pix_set method
   switch (dispnr) {
   case 0:
#use i2c(master, sda=PIN_DISPLAY_1_I2C_SDA, scl=PIN_DISPLAY_1_I2C_SCL)
      i2c_start();
      i2c_write(0x7A);         
      i2c_write(0b01100000+row); // start at row = row
      i2c_write(0b00000000+col); // start at column = col
      i2c_write(dsGetByte(dispnr, row, col));
      i2c_stop();
      break;

   case 1:
#use i2c(master, sda=PIN_DISPLAY_2_I2C_SDA, scl=PIN_DISPLAY_2_I2C_SCL)
      i2c_start();
      i2c_write(0x7A);         
      i2c_write(0b01100000+row); // start at row = row
      i2c_write(0b00000000+col); // start at column = col
      i2c_write(dsGetByte(dispnr, row, col));
      i2c_stop();
      break;

   case 2:
#use i2c(master, sda=PIN_DISPLAY_3_I2C_SDA, scl=PIN_DISPLAY_3_I2C_SCL)
      i2c_start();
      i2c_write(0x7A);         
      i2c_write(0b01100000+row); // start at row = row
      i2c_write(0b00000000+col); // start at column = col
      i2c_write(dsGetByte(dispnr, row, col));
      i2c_stop();
      break;

   case 3:
#use i2c(master, sda=PIN_DISPLAY_4_I2C_SDA, scl=PIN_DISPLAY_4_I2C_SCL)
      i2c_start();
      i2c_write(0x7A);         
      i2c_write(0b01100000+row); // start at row = row
      i2c_write(0b00000000+col); // start at column = col
      i2c_write(dsGetByte(dispnr, row, col));
      i2c_stop();
      break;

   case 4:
#use i2c(master, sda=PIN_DISPLAY_5_I2C_SDA, scl=PIN_DISPLAY_5_I2C_SCL)
      i2c_start();
      i2c_write(0x7A);         
      i2c_write(0b01100000+row); // start at row = row
      i2c_write(0b00000000+col); // start at column = col
      i2c_write(dsGetByte(dispnr, row, col));
      i2c_stop();
      break;

   case 5:
#use i2c(master, sda=PIN_DISPLAY_6_I2C_SDA, scl=PIN_DISPLAY_6_I2C_SCL)
      i2c_start();
      i2c_write(0x7A);         
      i2c_write(0b01100000+row); // start at row = row
      i2c_write(0b00000000+col); // start at column = col
      i2c_write(dsGetByte(dispnr, row, col));
      i2c_stop();
      break;

   }
   return 0;
}
   
/**
 * writes specified line from buffer to display; only columns in [start_col, end_col]
 * row = [0, MAX_ROW / 8], col = [0, MAX_COL]
 */
signed int displayLineSegUpdate(unsigned int dispnr, signed int row, signed int start_col, signed int end_col) {
   int col;

   if (! (insideSigned(row, start_col) && insideColSigned(end_col))) return -1;
//   if (! (inside(row, start_col) && insideCol(end_col))) return -1;

   // this code is partly taken from the old write_pix_set method
   switch (dispnr) {
   case 0:
#use i2c(master, sda=PIN_DISPLAY_1_I2C_SDA, scl=PIN_DISPLAY_1_I2C_SCL)
      i2c_start();
      i2c_write(0x7A);         
      i2c_write(0b01100000+row);
      i2c_write(0b00000000+start_col); // start at column = 0
      for(col = start_col; col <= end_col; col++) {
         i2c_write(dsGetByte(dispnr, row, col));
      }
      i2c_stop();
      break;

   case 1:
#use i2c(master, sda=PIN_DISPLAY_2_I2C_SDA, scl=PIN_DISPLAY_2_I2C_SCL)
      i2c_start();
      i2c_write(0x7A);         
      i2c_write(0b01100000+row);
      i2c_write(0b00000000+start_col); // start at column = 0
      for(col = start_col; col <= end_col; col++) {
         i2c_write(dsGetByte(dispnr, row, col));
      }
      i2c_stop();
      break;

   case 2:
#use i2c(master, sda=PIN_DISPLAY_3_I2C_SDA, scl=PIN_DISPLAY_3_I2C_SCL)
      i2c_start();
      i2c_write(0x7A);         
      i2c_write(0b01100000+row);
      i2c_write(0b00000000+start_col); // start at column = 0
      for(col = start_col; col <= end_col; col++) {
         i2c_write(dsGetByte(dispnr, row, col));
      }
      i2c_stop();
      break;

   case 3:
#use i2c(master, sda=PIN_DISPLAY_4_I2C_SDA, scl=PIN_DISPLAY_4_I2C_SCL)
      i2c_start();
      i2c_write(0x7A);         
      i2c_write(0b01100000+row);
      i2c_write(0b00000000+start_col); // start at column = 0
      for(col = start_col; col <= end_col; col++) {
         i2c_write(dsGetByte(dispnr, row, col));
      }
      i2c_stop();
      break;

   case 4:
#use i2c(master, sda=PIN_DISPLAY_5_I2C_SDA, scl=PIN_DISPLAY_5_I2C_SCL)
      i2c_start();
      i2c_write(0x7A);         
      i2c_write(0b01100000+row);
      i2c_write(0b00000000+start_col); // start at column = 0
      for(col = start_col; col <= end_col; col++) {
         i2c_write(dsGetByte(dispnr, row, col));
      }
      i2c_stop();
      break;

   case 5:
#use i2c(master, sda=PIN_DISPLAY_6_I2C_SDA, scl=PIN_DISPLAY_6_I2C_SCL)
      i2c_start();
      i2c_write(0x7A);         
      i2c_write(0b01100000+row);
      i2c_write(0b00000000+start_col); // start at column = 0
      for(col = start_col; col <= end_col; col++) {
         i2c_write(dsGetByte(dispnr, row, col));
      }
      i2c_stop();
      break;

   }
   return 0;
}
   
/**
 * writes specified line from buffer to display
 * row = [0, MAX_ROW / 8], col = [0, MAX_COL]
 */
signed int displayLineUpdate(unsigned int dispnr, signed int row) {
   if (!insideRowSigned(row)) return -1;

   return displayLineSegUpdate(dispnr, row, 0, MAX_COL);
}

/**
 * writes whole buffer to display
 */
signed int displayUpdate(unsigned int dispnr) {
   int row;
   for (row = 0; row <= MAX_ROW / 8; row++) {
      if (displayLineUpdate(dispnr, row) < 0) return -1;
   }
}








/******************************************************************************/
/* DISPLAY GRAPHICS HELPER METHDOS                                            */
/******************************************************************************/

/**
 * Uses Bresenham algorithm to draw a line.
 */
void bresenham(unsigned int dispnr, unsigned int x1, unsigned int y1, 
               unsigned int x2, unsigned int y2, int1 draw) {
   unsigned int x, y;
   signed int dx, dy;
   signed int incx, incy;
   signed int balance;

   if (x2 >= x1) {
      dx = x2 - x1;
      incx = 1;
   } else {
      dx = x1 - x2;
      incx = -1;
   }

   if (y2 >= y1) {
      dy = y2 - y1;
      incy = 1;
   } else {
      dy = y1 - y2;
      incy = -1;
   }

   x = x1;
   y = y1;

   if (dx >= dy) {
      dy <<= 1;
      balance = dy - dx;
      dx <<= 1;

      while (x != x2) {
         dsSetBit(dispnr, x, y, draw);
         if (balance >= 0) {
            y += incy;
            balance -= dx;
         }
         balance += dy;
         x += incx;
      }
      dsSetBit(dispnr, x, y, draw);
   } else {
      dx <<= 1;
      balance = dx - dy;
      dy <<= 1;

      while (y != y2) {
         dsSetBit(dispnr, x, y, draw);
         if ( balance >= 0 ) {
            x += incx;
            balance -= dy;
         }
         balance += dx;
         y += incy;
      }
      dsSetBit(dispnr, x, y, draw);
   }
}


// TODO does not work for most parameters
/**
 * Uses Bresenham algorithm to draw an ellipse.
 */
/*
   void bresenham_ellipse(unsigned int dispnr, unsigned int xc, unsigned int yc, 
   unsigned int xr, unsigned int yr, int1 draw) {
   signed int x, y, d;
   x = 0;
   y = yr;
   d = 2 * (1 - yr);

   while(y > 0) {
   // only draw pixels on display
   if (insideSigned(xc+x, yc+y)) dsSetBit(dispnr, xc + x, yc + y, draw);
   if (insideSigned(xc+x, yc-y)) dsSetBit(dispnr, xc + x, yc - y, draw);
   if (insideSigned(xc-x, yc+y)) dsSetBit(dispnr, xc - x, yc + y, draw);
   if (insideSigned(xc-x, yc-y)) dsSetBit(dispnr, xc - x, yc - y, draw);

   if(d + y > 0) {
   y -= 1;
   d -= (2 * y * xr / yr) - 1;
   }
   if(x > d) {
   x += 1;
   d += (2 * x) + 1;
   }
   }
   }
*/





/******************************************************************************/
/* HIGH LEVEL DISPLAY ACCESS METHODS                                          */
/******************************************************************************/


// --------------------------- SEVERAL DISPLAYS ----------------------------- //
/**
 * Clears the given display.
 */
signed int bt_clear_display(unsigned int dispnr, int1 background) {
  unsigned int r, c;
  signed int ret = 0;
  unsigned char back = 0x00;

  if (background > 0) back = 0xFF;

  for (r = 0; r <= MAX_ROW / 8; r++)
    for (c = 0; c <= MAX_COL; c++)
      if (dsSetByte(dispnr, r, c, back) < 0) ret = -1;

  displayUpdate(dispnr);
  return ret;
}


// TODO implement for all directions
signed int bt_scroll_rect_display(unsigned dispnr, signed int start_row, 
      signed int start_col, signed int end_row, signed int end_col, 
      unsigned int direction, unsigned int speed, unsigned int steps) {
   signed int row, col, swapper;
   //unsigned byte region[DISP_SIZE_X][DISP_SIZE_Y];
   signed int new_row, new_col, step;
   //unsigned char xtx[15];
    

   // checks
   if (! (inside(start_row, start_col) && inside(end_row, end_col))) return -1;
   //   if (direction != MOVEDOWN) return -2; // currently only down is supported
   if (direction != MOVEDOWN && direction != MOVEUP &&
         direction != MOVELEFT && direction != MOVERIGHT) return -2;

   // ensure start < end coordinates
   if (start_row > end_row) { swapper = start_row; start_row = end_row; end_row = swapper; }
   if (start_col > end_col) { swapper = start_col; start_col = end_col; end_col = swapper; }

   //for (row = start_row; row <= end_row; row++) {
   //   for (col = start_col; col <= end_col; col++) {
   //      region[row][col] = dsGetBit(dispnr, row, col);
   //   }
   //}

   if (direction == MOVEDOWN)
      for (step = 0; step < steps; step++) {
         //sprintf(xtx, "step: %u", step);
         //bt_write_string_display(1, 0, 0, xtx);

            if (end_row + speed > MAX_ROW) end_row = MAX_ROW - speed;

         // copy values from rectangle to new place
         for (row = end_row +speed; row >= start_row +speed; row--) {
            for (col = start_col; col <= end_col; col++) {
               dsSetBit(dispnr, row, col, dsGetBit(dispnr, row -speed, col));
            }
         }
         // remove first <speed> lines
         for (row = start_row; row < start_row +speed; row++) {
            for (col = start_col; col <= end_col; col++) {
               dsSetBit(dispnr, row, col, 0);
            }
         }
         
         end_row += speed;
         
         //      // TODO more efficient
         //      displayUpdate(dispnr);
         // update lines from (old) start_row till (new) end_row
         displayLineSegUpdate(dispnr, start_row / 8, start_col, end_col);
         for (row = start_row / 8 +1; row <= end_row / 8 -1; row++)
            displayLineSegUpdate(dispnr, row, start_col, end_col);
         displayLineSegUpdate(dispnr, end_row / 8, start_col, end_col);
         
         start_row += speed;
      }

   else if (direction == MOVEUP)
      for (step = 0; step < steps; step++) {

            if (((unsigned int)(start_row - speed)) < 0) start_row = speed;

         // copy values from rectangle to new place
         for (row = start_row -speed; row <= end_row -speed; row++) {
            for (col = start_col; col <= end_col; col++) {
               dsSetBit(dispnr, row, col, dsGetBit(dispnr, row +speed, col));
            }
         }
         // remove first <speed> lines
         for (row = end_row; row > end_row -speed; row--) {
            for (col = start_col; col <= end_col; col++) {
               dsSetBit(dispnr, row, col, 0);
            }
         }
        
         start_row -= speed;

         displayLineSegUpdate(dispnr, start_row / 8, start_col, end_col);
         for (row = start_row / 8 +1; row <= end_row / 8 -1; row++)
            displayLineSegUpdate(dispnr, row, start_col, end_col);
         displayLineSegUpdate(dispnr, end_row / 8, start_col, end_col);
        
         end_row -= speed;
      }
    
   else if (direction == MOVERIGHT)
      for (step = 0; step < steps; step++) {

            if (end_col + speed > MAX_COL) end_col = MAX_COL - speed;

         // copy values from rectangle to new place
         for (col = end_col +speed; col >= start_col +speed; col--) {
            for (row = start_row; row < end_row; row++) {
               dsSetBit(dispnr, row, col, dsGetBit(dispnr, row, col -speed));
            }
         }
         // remove first <speed> lines
         for (col = start_col; col < start_col +speed; col++) {
            for (row = start_row; row < end_row; row++) {
               dsSetBit(dispnr, row, col, 0);
            }
         }
         
         end_col += speed;
         
            displayUpdate(0);
//         displayLineSegUpdate(dispnr, start_row / 8, start_col, end_col);
//         for (row = start_row / 8 +1; row <= end_row / 8 -1; row++)
//            displayLineSegUpdate(dispnr, row, start_col, end_col);
//         displayLineSegUpdate(dispnr, end_row / 8, start_col, end_col);
         
         start_col += speed;
      }

   else // if (direction == MOVELEFT)
      for (step = 0; step < steps; step++) {

            if (((unsigned int)(start_col - speed)) < 0) start_col = speed;

         // copy values from rectangle to new place
         for (col = start_col -speed; col <= end_col -speed; col++) {
            for (row = start_row; row < end_row; row++) {
               dsSetBit(dispnr, row, col, dsGetBit(dispnr, row, col +speed));
            }
         }
         // remove first <speed> lines
         for (col = end_col; col > end_col -speed; col--) {
            for (row = start_row; row < end_row; row++) {
               dsSetBit(dispnr, row, col, 0);
            }
         }
        
         start_col -= speed;

            displayUpdate(0);
//         displayLineSegUpdate(dispnr, start_row / 8, start_col, end_col);
//         for (row = start_row / 8 +1; row <= end_row / 8 -1; row++)
//            displayLineSegUpdate(dispnr, row, start_col, end_col);
//         displayLineSegUpdate(dispnr, end_row / 8, start_col, end_col);
        
         end_col -= speed;
      }
   // "end switch case" MOVE...
}


unsigned int getNumberOfLines(char* text, int rotation, int1 wordwrap) {
  unsigned int len, i, nroflines, textwidth, lastspacepos;
  signed int charlinecnt;
  len = strlen(text);
  if (len == 0) return 0;

  if (rotation == DEG0 || rotation == DEG180) {
    // wide side, text width = 16
    textwidth = 16;
  } else { // if (rotation == DEG90 || rotation == DEG270)
    // small side, text width = 6
    textwidth = 6;
  }

  if (wordwrap == 0) {
    // no word wrap
    return (len / textwidth) +1;
  } else {
    // take word wrap into account
    nroflines = 1;
    lastspacepos = 0;
    charlinecnt = 0;
    for(i = 0; i < len; i++) {
      if (text[i] == ' ') {
        lastspacepos = i;
        charlinecnt++;
      } else if (charlinecnt >= textwidth) {
        // wrap
        nroflines++;
        if (i - lastspacepos <= textwidth) {
          // can/must move last word to next line
          charlinecnt = i - lastspacepos -1;
        } else {
          // must split the word in two (larger than <textwidth> characters)
          charlinecnt = 0;
        }
      } else {
        charlinecnt++;
      }
    }
  }
  
  return nroflines;
}


/**
 * Any content present on the display is scrolled down one line.
 * Then the given text is written to the first line of the display.
 */
signed int bt_scroll_text_display(unsigned int dispnr, char* text, int rotation, int1 draw, int1 wordwrap) {
  unsigned int nroflines, speed;
  speed = 2;

  if (rotation != DEG0 && rotation != DEG90 && 
      rotation != DEG180 && rotation != DEG270) return -1;

  nroflines = getNumberOfLines(text, rotation, wordwrap);

  if (rotation == DEG0)
    bt_scroll_rect_display(dispnr, 0, 0, MAX_ROW, MAX_COL, MOVEDOWN, speed, 8*nroflines/speed);
  else if (rotation == DEG90)
    bt_scroll_rect_display(dispnr, 0, 0, MAX_ROW, MAX_COL, MOVELEFT, speed, 8*nroflines/speed);
  else if (rotation == DEG180)
    bt_scroll_rect_display(dispnr, 0, 0, MAX_ROW, MAX_COL, MOVEUP, speed, 8*nroflines/speed);
  else // if (rotation == DEG270)
    bt_scroll_rect_display(dispnr, 0, 0, MAX_ROW, MAX_COL, MOVERIGHT, speed, 8*nroflines/speed);

  bt_text_ww_display(dispnr, text, 0, 0, rotation, draw, wordwrap);
}

/**
 * Given Text is scrolled in one line either horizontally or vertically.
 */
signed int bt_scroll_text_display2(unsigned int dispnr, 
               char* text, 
               unsigned int orientation, 
               signed int row, signed int col, 
               int rotation, 
               int1 draw, 
               int1 wordwrap, 
               unsigned int speed,
               unsigned int nrsteps) {

  unsigned int i, offset;
  unsigned long len;
  char * disptext;
  char mess[30];
  
  if (rotation != DEG0 && rotation != DEG90 && 
      rotation != DEG180 && rotation != DEG270) return -1;
  
  if (rotation == DEG180) {
    disptext = text;
    bt_text_ww_display(dispnr, disptext, row, col, rotation, draw, wordwrap);
    
    if (orientation == HORIZONTAL_SCROLL) {
      for (i = 0; i < nrsteps; i++) {
        col = col - speed;
        bt_scroll_rect_display(dispnr, 
                MAX_ROW - row - BT_CHARACTER_PIXEL_HEIGHT, 
                0, 
                MAX_ROW - row, 
                MAX_COL, 
                MOVERIGHT, 
                speed, 1);
        bt_text_ww_display(dispnr, disptext, row, col, rotation, draw, wordwrap);
        if (col < -10) {
          offset = abs(col) / BT_CHARACTER_PIXEL_WIDTH;
          if (offset < strlen(disptext)) {
            disptext = &disptext[offset];
            col = col + (offset * BT_CHARACTER_PIXEL_WIDTH);
            // sprintf(mess, "%D ", col);
            // bt_text_ww_display(dispnr, mess, 0, 0, rotation, draw, wordwrap);
          }
        }
      }
    } else if (orientation == VERTICAL_SCROLL) {
      bt_text_ww_display(dispnr, disptext, row, col, rotation, draw, wordwrap);
      for (i = 0; i < nrsteps; i++) {
        bt_scroll_rect_display(dispnr, 
                0,   
                0, 
                MAX_ROW, 
                MAX_COL, MOVEUP, 
                speed, 1);
   
        row = row + speed;
        if (row < 0) {
          bt_text_ww_display(dispnr, disptext, row, col, rotation, draw, wordwrap);
        }
      }
    } 
  } 
}

signed int bt_text_ww_display_buf(unsigned int dispnr, char* text, signed int row, signed int col, int rotation, int1 draw, int1 wordwrap) {
  signed int lastcol, lastrow;
  signed int start_row, start_col;
  signed int conv_r, conv_c, cor_r, cor_c;
  unsigned byte byteletter[6];
  unsigned int c, r, i;
  signed byte rowRotSign, colRotSign;
  unsigned int len, charcnt, nroflines, textwidth, lastspacepos, charlinecnt;

  if (rotation != DEG0 && rotation != DEG90 && 
      rotation != DEG180 && rotation != DEG270) return -1;

  len = strlen(text);

  // lastrow / lastcol are needed because a letter is defined by a 8x6 bit matrix
  // by rotating, this can be changed to 6x8
  lastrow = 6;
  if (rotation == DEG0 || rotation == DEG180) lastrow = 8;
  lastcol = 14 - lastrow; // if lastrow == 6 then lastcol = 8 and v.v.

  rowRotSign = 1;
  if (rotation == DEG90 || rotation == DEG180) rowRotSign = -1;
  colRotSign = 1;
  if (rotation == DEG180 || rotation == DEG270) colRotSign = -1;

  convert_coords(rotation, row, col, &start_row, &start_col, MAX_ROW, MAX_COL);
    
  lastspacepos = 0;
  charlinecnt = 0;

  if (rotation == DEG0 || rotation == DEG180) {
     // wide side, text width = 16
     textwidth = 16;
  } else { // if (rotation == DEG90 || rotation == DEG270)
     // small side, text width = 6
     textwidth = 6;
  }

  // write each letter
  for(i = 0; i < len; i++) {

    char letter;
    letter = text[i];
    // get byte[] representation
   get_letter(letter, byteletter);

    if (wordwrap == 0) {
       // no word wrap
       // if the character would not fit in the line, go to start of next line
       if (rotation == DEG90 && (signed int)(start_row - BT_CHARACTER_PIXEL_WIDTH +1) < 0) {
          start_row = MAX_ROW; start_col += BT_CHARACTER_PIXEL_HEIGHT;
       } else if (rotation == DEG270 && start_row + BT_CHARACTER_PIXEL_WIDTH -1 > MAX_ROW) {
          start_row = 0; start_col -= BT_CHARACTER_PIXEL_HEIGHT;
       } else if (rotation == DEG0 && start_col + BT_CHARACTER_PIXEL_WIDTH -1 > MAX_COL) {
          start_col = 0; start_row += BT_CHARACTER_PIXEL_HEIGHT;
       } else if (rotation == DEG180 && (signed int)(start_col - BT_CHARACTER_PIXEL_WIDTH +1) < 0) {
          start_col = MAX_COL; start_row -= BT_CHARACTER_PIXEL_HEIGHT;
       }
/*       for 1 line scrolling:
         // Assume outside of display range
         // No wrapping whatsoever, implies no need to continue
         if (rotation == DEG180 && (signed int)(start_col - BT_CHARACTER_PIXEL_WIDTH +1) < 0) {
            break;
         }
*/

    } else {
       // take word wrap into account

       // find out how many characters there are till the end or a space
       charcnt = 0;
       while (charcnt+i < len && text[charcnt+i] != ' ') charcnt++;

       if (charlinecnt > 0 && charlinecnt + charcnt > textwidth) {
          // need to wrap the word into the next line
          if (rotation == DEG90) {
             start_row = MAX_ROW; start_col += BT_CHARACTER_PIXEL_HEIGHT;
          } else if (rotation == DEG270) {
             start_row = 0; start_col -= BT_CHARACTER_PIXEL_HEIGHT;
          } else if (rotation == DEG0) {
             start_col = 0; start_row += BT_CHARACTER_PIXEL_HEIGHT;
          } else if (rotation == DEG180) {
             start_col = MAX_COL; start_row -= BT_CHARACTER_PIXEL_HEIGHT;
          }
          if (letter == ' ') {
             // don't print spaces at the beginning of a line
             charlinecnt = 0;
             continue;
          } else {
             // the current letter will be printed in the (new) current line
             charlinecnt = 1;
          }
       } else {
          charlinecnt++;
       }
    }

    // write charyter byte[]
    for (c = 0; c < lastcol; c++) {
      for (r = 0; r < lastrow; r++) {
        convert_coords(rotation, r, c, &conv_r, &conv_c, 7, 5);
   
        if (bit_test(byteletter[conv_c], conv_r)) {
          cor_r = r;
          cor_c = c;
          // prevent the letters from being displayed mirrowed
          if (rotation == DEG90) cor_c = lastcol - c;
          if (rotation == DEG180) { cor_c = lastcol - c; cor_r = lastrow - r; }
          if (rotation == DEG270) cor_r = lastrow - r;

          /*
            if (rotation == DEG90 && start_row + rowRotSign*cor_r < 0) {
            start_row = MAX_ROW; start_col += BT_CHARACTER_PIXEL_HEIGHT;
            } else if (rotation == DEG270 && start_row + rowRotSign*cor_r > MAX_ROW) {
            start_row = 0; start_col -= BT_CHARACTER_PIXEL_HEIGHT;
            } else if (rotation == DEG0 && start_col + colRotSign*cor_c > MAX_COL) {
            start_col = 0; start_row += BT_CHARACTER_PIXEL_HEIGHT;
            } else if (rotation == DEG180 && start_col + colRotSign*cor_c < 0) {
            start_col = MAX_COL; start_row -= BT_CHARACTER_PIXEL_HEIGHT;
            }
          */

          // (out of bounds are ignored)
          dsSetBit(dispnr, start_row + rowRotSign*cor_r, start_col + colRotSign*cor_c, draw);
        }
      }
    }
   
    // shift "cursor" to next character position
    if      (rotation == DEG90)  start_row -= 6;
    else if (rotation == DEG270) start_row += 6;
    else if (rotation == DEG0)   start_col += 6;
    else                         start_col -= 6;
  }

}
/**
 * Writes given text on a given display. Can choose word wrapping.
 * \param dispnr the number of the display [0,5] to write on
 */
signed int bt_text_ww_display(unsigned int dispnr, char* text, signed int row, signed int col, int rotation, int1 draw, int1 wordwrap) {

    bt_text_ww_display_buf(dispnr, text, row, col, rotation, draw, wordwrap);
  // TODO check if this can be done more efficient
  return displayUpdate(dispnr);
/*
  if (rotation == DEG0) {
   if (len > textwidth)
      displayLineSegUpdate(dispnr, row, col, MAX_COL);
    else 
      displayLineSegUpdate(dispnr, row, col, mathmin(MAX_COL, len*BT_CHARACTER_PIXEL_WIDTH));

    // update the rest of the lines (start_row will now be the last row written)
    for (r = row + BT_CHARACTER_PIXEL_HEIGHT; r <= start_row; r += BT_CHARACTER_PIXEL_HEIGHT)
      displayLineUpdate(dispnr, row);

  } else {
    // TODO check if this can be done more efficient
    return displayUpdate(dispnr);
  }
*/
}

/** 
 * Clears the display buffer without updating the display
 * \param dispnr the number of the display [0,5] to write on
 */

void bt_clear_buffer(unsigned int dispnr)
{
  unsigned char back = 0x00;
  unsigned int c, r;

  for (r = 0; r <= MAX_ROW / 8; r++)
    for (c = 0; c <= MAX_COL; c++)
      dsSetByte(dispnr, r, c, back);
}

/**
 * Writes given text on a given display buffer without updating.
 */
signed int bt_text_display_big_buf(unsigned int dispnr, char* text, signed int row, signed int col, int rotation, int1 draw) {
  const unsigned int LETTER_WIDTH = 12;
  const unsigned int LETTER_HEIGHT = 16;
  const unsigned int LETTERS_PER_LINE = 96 / LETTER_WIDTH;
  const unsigned int LINES_PER_DISPLAY = 40 / LETTER_HEIGHT;
  signed int lastcol, lastrow;
  signed int start_row, start_col;
  signed int conv_r, conv_c, cor_r, cor_c;
  unsigned byte byteletter[6];
  unsigned int c, r, i;
  unsigned int c2, r2;
  signed byte rowRotSign, colRotSign;
  unsigned int len, charcnt, nroflines, lastspacepos, charlinecnt;


  if (rotation != DEG0 && rotation != DEG90 && 
      rotation != DEG180 && rotation != DEG270) return -1;

  // compensate that all coordinates will be doubled
  row /= 2;
  col /= 2;

  len = strlen(text);

  // lastrow / lastcol are needed because a letter is defined by a 8x6 bit matrix
  // by rotating, this can be changed to 6x8
  lastrow = 6;
  if (rotation == DEG0 || rotation == DEG180) lastrow = 8;
  lastcol = 14 - lastrow; // if lastrow == 6 then lastcol = 8 and v.v.

  rowRotSign = 1;
  if (rotation == DEG90 || rotation == DEG180) rowRotSign = -1;
  colRotSign = 1;
  if (rotation == DEG180 || rotation == DEG270) colRotSign = -1;

  convert_coords(rotation, row, col, &start_row, &start_col, MAX_ROW, MAX_COL);
    
  lastspacepos = 0;
  charlinecnt = 0;

  // write each letter
  for(i = 0; i < len; i++) {

    char letter;
    letter = text[i];
    // get byte[] representation
    get_letter(letter, byteletter);
   
   /*
   if (rotation == DEG0 || rotation == DEG180) {
      // wide side, text width = LETTERS_PER_LINE
      textwidth = LETTERS_PER_LINE;
   } else { // if (rotation == DEG90 || rotation == DEG270)
      // small side, text width = LINES_PER_DISPLAY
      textwidth = LINES_PER_DISPLAY;
   }
   */

   // if the character would not fit in the line, go to start of next line
   if (rotation == DEG90 && (signed int)(2*start_row - LETTER_WIDTH +1) < 0) {
      // TODO
      //start_row = MAX_ROW; start_col += BT_CHARACTER_PIXEL_HEIGHT;
   } else if (rotation == DEG270 && 2*start_row + LETTER_WIDTH -1 > MAX_ROW) {
      // TODO
      //start_row = 0; start_col -= BT_CHARACTER_PIXEL_HEIGHT;

   } else if (rotation == DEG0 && 2*start_col + LETTER_WIDTH -1 > MAX_COL) {
///      start_col = 0; start_row += BT_CHARACTER_PIXEL_HEIGHT;
      start_col = 0; start_row += BT_CHARACTER_PIXEL_HEIGHT-1;
   } else if (rotation == DEG180 && (signed int)(2*start_col-MAX_COL - LETTER_WIDTH +1) < 0) {
      start_col = MAX_COL; start_row -= BT_CHARACTER_PIXEL_HEIGHT;
   }

    // write character byte[]
    for (c = 0; c < lastcol; c++) {
///      for (r = 0; r < lastrow; r++) {
      for (r = 0; r < lastrow-1; r++) {
//        r = r2;
//        c = c2;
        if (letter == 'g' || letter == 'M' || letter == '+' || letter == '*' 
            || letter == '4' || letter == '$' || letter == '%' || letter == ':' || letter == ';') {
          if (r == 0) 
            continue;
        } else if (letter == '3' || letter == '5' || letter == '6' 
            || letter == '7' || letter == 'i') {
          if (r == 4)
            continue;
        } else if (letter == 'D' || letter == '1' || letter == '(' || letter == ')') {
          if (r == 3)
            continue;
        } else if (letter == '2') {
          if (r == 2)
            continue;
        } else if (r == 1) {
          continue;
        }

        convert_coords(rotation, r, c, &conv_r, &conv_c, 
           BT_CHARACTER_PIXEL_HEIGHT-1, BT_CHARACTER_PIXEL_WIDTH-1);
   
        if (bit_test(byteletter[conv_c], conv_r)) {
          cor_r = r;
          cor_c = c;
          // prevent the letters from being displayed mirrowed
          if (rotation == DEG90) cor_c = lastcol - c;
          if (rotation == DEG180) { cor_c = lastcol - c; cor_r = lastrow - r; }
          if (rotation == DEG270) cor_r = lastrow - r;

        if (letter == 'g' || letter == 'M' || letter == '+' || letter == '*' 
            || letter == '4' || letter == '$' || letter == '%' || letter == ':' || letter == ';') {
          if (r > 0) 
            cor_r--;
        } else if (letter == '3' || letter == '5' || letter == '6' 
            || letter == '7' || letter == 'i') {
          if (r > 4)
            cor_r--;
        } else if (letter == 'D' || letter == '1' || letter == '(' || letter == ')') {
          if (r > 3)
          cor_r--;
        } else if (letter == '2') {
          if (r > 2)
          cor_r--;
        } else if (r > 1) {
          cor_r--;
        }

          /*
            if (rotation == DEG90 && start_row + rowRotSign*cor_r < 0) {
            start_row = MAX_ROW; start_col += BT_CHARACTER_PIXEL_HEIGHT;
            } else if (rotation == DEG270 && start_row + rowRotSign*cor_r > MAX_ROW) {
            start_row = 0; start_col -= BT_CHARACTER_PIXEL_HEIGHT;
            } else if (rotation == DEG0 && start_col + colRotSign*cor_c > MAX_COL) {
            start_col = 0; start_row += BT_CHARACTER_PIXEL_HEIGHT;
            } else if (rotation == DEG180 && start_col + colRotSign*cor_c < 0) {
            start_col = MAX_COL; start_row -= BT_CHARACTER_PIXEL_HEIGHT;
            }
          */

          // (out of bounds are ignored)
          if (rotation == DEG0) {
            dsSetBit(dispnr, 2*(start_row + cor_r), 2*(start_col + cor_c), draw);
            dsSetBit(dispnr, 2*(start_row + cor_r)+1, 2*(start_col + cor_c), draw);
            dsSetBit(dispnr, 2*(start_row + cor_r), 2*(start_col + cor_c)+1, draw);
            dsSetBit(dispnr, 2*(start_row + cor_r)+1, 2*(start_col + cor_c)+1, draw);
          } else if (rotation == DEG90) {
            // TODO
          } else if (rotation == DEG180) {
            dsSetBit(dispnr, 2*(start_row + rowRotSign*cor_r)-MAX_ROW, 2*(start_col + colRotSign*cor_c)-MAX_COL, draw);
            dsSetBit(dispnr, 2*(start_row + rowRotSign*cor_r)+1-MAX_ROW, 2*(start_col + colRotSign*cor_c)-MAX_COL, draw);
            dsSetBit(dispnr, 2*(start_row + rowRotSign*cor_r)-MAX_ROW, 2*(start_col + colRotSign*cor_c)+1-MAX_COL, draw);
            dsSetBit(dispnr, 2*(start_row + rowRotSign*cor_r)+1-MAX_ROW, 2*(start_col + colRotSign*cor_c)+1-MAX_COL, draw);
          } else { // if (rotation == DEG270)
            // TODO
          }
        }
      }
    }
   
    // shift "cursor" to next character position
    if      (rotation == DEG90)  start_row -= BT_CHARACTER_PIXEL_WIDTH;
    else if (rotation == DEG270) start_row += BT_CHARACTER_PIXEL_WIDTH;
    else if (rotation == DEG0)   start_col += BT_CHARACTER_PIXEL_WIDTH;
    else                         start_col -= BT_CHARACTER_PIXEL_WIDTH;
  }
    
}
/**
 * Writes given text on a given display. Writes BIG letters (twice as big as normal).
 * \param dispnr the number of the display [0,5] to write on
 */
signed int bt_text_display_big(unsigned int dispnr, char* text, signed int row, signed int col, int rotation, int1 draw) {
    
    bt_text_display_big_buf(dispnr, text, row, col, rotation, draw);

  // TODO check if this can be done more efficient
  return displayUpdate(dispnr);
/*
  if (rotation == DEG0) {
   if (len > textwidth) {
      displayLineSegUpdate(dispnr, row, col, MAX_COL);
      displayLineSegUpdate(dispnr, row+1, col, MAX_COL);
    } else {
      displayLineSegUpdate(dispnr, row, col, mathmin(MAX_COL, len*LETTER_WIDTH));
      displayLineSegUpdate(dispnr, row+1, col, mathmin(MAX_COL, len*LETTER_WIDTH));
    }
    
    // update the rest of the lines (start_row will now be the last row written)
    for (r = row + LETTER_HEIGHT; r <= start_row; r += LETTER_HEIGHT)
      displayLineUpdate(dispnr, row);

  } else {
    // TODO check if this can be done more efficient
    return displayUpdate(dispnr);
  }
*/
}


/**
 * Writes given text on a given display. No word wrapping.
 * \param dispnr the number of the display [0,5] to write on
 */
signed int bt_text_display(unsigned int dispnr, char* text, signed int row, signed int col, int rotation, int1 draw) {
  bt_text_ww_display(dispnr, text, row, col, rotation, draw, 0);
}


signed int bt_text_display_buf(unsigned int dispnr, char* text, signed int row, signed int col, int rotation, int1 draw) {
  bt_text_ww_display_buf(dispnr, text, row, col, rotation, draw, 0);
}

/**
 * draws an ellipse with center at (row, col) and given x and y radius
 * row in [0,MAX_ROW], column in [0,MAX_COL]
 * draw = 1 means draw ellipse, draw = 0 means erase ellipse
 */
/*
   signed int bt_ellipse_display(unsigned int dispnr, unsigned int center_row, unsigned int center_col, 
   unsigned int x_radius, unsigned int y_radius, int1 draw) {

  bresenham_ellipse(dispnr, center_row, center_col, x_radius, y_radius, draw);

  // TODO maybe can do better ...
  return displayUpdate(dispnr);
   }
*/

/**
 * draws a circle with center at (row, col) and given radius
 * row in [0,MAX_ROW], column in [0,MAX_COL]
 * draw = 1 means draw circle, draw = 0 means erase circle
 */
/*
   signed int bt_circle_display(unsigned int dispnr, unsigned int row, unsigned int col, unsigned int radius, int1 draw) {
  // TODO could implement faster code for circle
  return bt_ellipse_display(dispnr, row, col, radius, radius, draw);
   }
*/


/**
 * draws a line from (start_row, start_col) to (end_row, end_col)
 * rows in [0,MAX_ROW], columns in [0,MAX_COL]
 * draw = 1 means draw line, draw = 0 means erase line
 */
signed int bt_line_display(unsigned int dispnr, unsigned int start_row, unsigned int start_col, 
                           unsigned int end_row, unsigned int end_col, int1 draw) {

  if (! (inside(start_row, start_col) && inside(end_row, end_col))) return -1;

  bresenham(dispnr, start_row, start_col, end_row, end_col, draw);

  // TODO check the more efficient version
  //   for (row = start_row / 8; row <= end_row / 8; row++) {
  //      displayLineSegUpdate(row, start_col, end_col);
  //   }
  return displayUpdate(dispnr);

}

/**
 * draws a box from (start_row, start_col) to (end_row, end_col)
 * rows in [0,MAX_ROW], columns in [0,MAX_COL]
 * draw = 1 means draw box, draw = 0 means erase box
 */
signed int bt_box_display(unsigned int dispnr, unsigned int start_row, unsigned int start_col, 
                          unsigned int end_row, unsigned int end_col, int1 draw) {
  unsigned int r, c, temp;

  if (! (inside(start_row, start_col) && inside(end_row, end_col))) return -1;

  temp = mathmin(start_row, end_row);
  end_row = mathmax(start_row, end_row);
  start_row = temp;
  temp = mathmin(start_col, end_col);
  end_col = mathmax(start_col, end_col);
  start_col = temp;


  // left
  for (r = start_row; r <= end_row; r++)
    if (dsSetBit(dispnr, r, start_col, draw) < 0) return -1;
  // right
  for (r = start_row; r <= end_row; r++)
    if (dsSetBit(dispnr, r, end_col, draw)) return -1;
  // top
  for (c = start_col; c <= end_col; c++)
    if (dsSetBit(dispnr, start_row, c, draw)) return -1;
  // bottom
  for (c = start_col; c <= end_col; c++)
    if (dsSetBit(dispnr, end_row, c, draw)) return -1;

  if (displayLineSegUpdate(dispnr, start_row / 8, start_col, end_col)) return -1;
  for (r = start_row / 8 +1; r <= end_row / 8 -1; r++) {
    if (displayByteUpdate(dispnr, r, start_col)) return -1;
    if (displayByteUpdate(dispnr, r, end_col)) return -1;
  }
  if (displayLineSegUpdate(dispnr, end_row / 8, start_col, end_col)) return -1;

  return 0;
}

/**
 * draws a vertical line [start_row, end_row] (each in [0,MAX_ROW]) in column col ([0,MAX_COL])
 * draw = 1 means draw line, draw = 0 means erase line
 */
signed int bt_vLine_display(unsigned int dispnr, unsigned int start_row, unsigned int col,
                            unsigned int end_row, int1 draw) {
  unsigned int r;

  if (! (inside(start_row, col) && insideRow(end_row))) return -1;

  // add line to buffer
  for (r = start_row; r <= end_row; r++) {
    if (dsSetBit(dispnr, r, col, draw) < 0) return -1;
    //dSetByte(r/8, col, 0xFF);
  }

  for (r = start_row / 8; r <= end_row / 8; r++) {
    if (displayLineSegUpdate(dispnr, r, col, col) < 0) return -1;
  }

  return 0;
}

/**
 * draws a horizontal line in row=[0,MAX_ROW] from start_col to end_col ([0,MAX_COL])
 * draw = 1 means draw line, draw = 0 means erase line
 */
signed int bt_hLine_display(unsigned int dispnr, unsigned int row, unsigned int start_col, 
                            unsigned int end_col, int1 draw) {
  unsigned int c;

  if (! (inside(row, start_col) && insideCol(end_col))) return -1;

  // add line to buffer
  for (c = start_col; c <= end_col; c++) {
    if (dsSetBit(dispnr, row, c, draw) < 0) return -1;
  }

  return displayLineSegUpdate(dispnr, row / 8, start_col, end_col);
}

/**
 * inverts every pixel on the display
 */
signed int bt_invert_display(unsigned int dispnr) {
  unsigned int r, c;
  signed int ret = 0;

  for (r = 0; r <= MAX_ROW / 8; r++) {
    for (c = 0; c <= MAX_COL; c++) {
      if (dsSetByte(dispnr, r, c, 255 - dsGetByte(dispnr, r, c)) < 0) ret = -1;
      //display[r][c] = 255 - display[r][c];
    }
  }
  if (displayUpdate(dispnr) < 0) return -1;

  return ret;
}


// TODO check whether this works
/*
   signed int bt_rotate_display(unsigned int dispnr, unsigned int start_row, unsigned int start_col, 
   unsigned int end_row, unsigned int end_col, int rotation) {
  unsigned int row, col, swapper;
  unsigned byte region[DISP_SIZE_X][DISP_SIZE_Y];
  unsigned int new_row, new_col;
    
  // checks
  if (! (inside(start_row, start_col) && inside(end_row, end_col))) return -1;
  if (rotation != DEG0 && rotation != DEG90 && rotation != DEG180 && rotation != DEG270) return -2;

  // ensure start < end coordinates
  if (start_row > end_row) { swapper = start_row; start_row = end_row; end_row = swapper; }
  if (start_col > end_col) { swapper = start_col; start_col = end_col; end_col = swapper; }

  //    region = calloc((end_row / 8 - start_row / 8 +1) * ((end_col - start_col) / 8 +1), 1);
  for (row = start_row; row <= end_row; row++) {
   for (col = start_col; col <= end_col; col++) {
   region[row][col] = dsGetBit(dispnr, row, col);
   }
  }
  for (row = start_row; row <= end_row; row++) {
   for (col = start_col; col <= end_col; col++) {
   if (convert_coords(rotation, row, col, &new_row, &new_col, MAX_ROW, MAX_COL) >= 0) {
   dsSetBit(dispnr, new_row, new_col, region[row][col]);
   }
   }
  }

  // TODO more efficient
  displayUpdate(dispnr);
   }
*/




// --------------------------- STANDARD DISPLAY ----------------------------- //

/**
 * Clears the standard display (0).
 * \param background 0 is black (pixel switched off)
 */
signed int bt_clear(int1 background) {
  return bt_clear_display(STD_DISP, background);
}


/**
 * Writes given text on the standard display (0). Can choose word wrapping.
 * draw = 1 means draw text, draw = 0 means erase text
 */
signed int bt_text_ww(char* text, unsigned int row, unsigned int col, int rotation, int1 draw, int1 wordwrap) {
  return bt_text_ww_display(STD_DISP, text, row, col, rotation, draw, wordwrap);
}

/**
 * Writes given text on the standard display (0).
 * draw = 1 means draw text, draw = 0 means erase text
 */
signed int bt_text(char* text, unsigned int row, unsigned int col, int rotation, int1 draw) {
  bt_text_ww(text, row, col, rotation, draw, 0);
}


/**
 * draws an ellipse with center at (row, col) and given x and y radius
 * row in [0,MAX_ROW], column in [0,MAX_COL]
 * draw = 1 means draw ellipse, draw = 0 means erase ellipse
 */
/*
   signed int bt_ellipse(unsigned int center_row, unsigned int center_col, 
   unsigned int x_radius, unsigned int y_radius, int1 draw) {

  return bt_ellipse_display(STD_DISP, center_row, center_col, x_radius, y_radius, draw);
   }
*/

/**
 * draws a circle with center at (row, col) and given radius
 * row in [0,MAX_ROW], column in [0,MAX_COL]
 * draw = 1 means draw circle, draw = 0 means erase circle
 */
/*
   signed int bt_circle(unsigned int row, unsigned int col, unsigned int radius, int1 draw) {
  return bt_circle_display(STD_DISP, row, col, radius, draw);
   }
*/


/**
 * draws a line from (start_row, start_col) to (end_row, end_col)
 * rows in [0,MAX_ROW], columns in [0,MAX_COL]
 * draw = 1 means draw line, draw = 0 means erase line
 */
signed int bt_line(unsigned int start_row, unsigned int start_col, unsigned int end_row,
                   unsigned int end_col, int1 draw) {

  return bt_line_display(STD_DISP, start_row, start_col, end_row, end_col, draw);
}

/**
 * draws a box from (start_row, start_col) to (end_row, end_col)
 * rows in [0,MAX_ROW], columns in [0,MAX_COL]
 * draw = 1 means draw box, draw = 0 means erase box
 */
signed int bt_box(unsigned int start_row, unsigned int start_col, unsigned int end_row,
                  unsigned int end_col, int1 draw) {
  return bt_box_display(STD_DISP, start_row, start_col, end_row, end_col, draw);
}

/**
 * draws a vertical line [start_row, end_row] (each in [0,MAX_ROW]) in column col ([0,MAX_COL])
 * draw = 1 means draw line, draw = 0 means erase line
 */
signed int bt_vLine(unsigned int start_row, unsigned int col, unsigned int end_row, int1 draw) {
  return bt_vLine_display(STD_DISP, start_row, col, end_row, draw);
}

/**
 * draws a horizontal line in row=[0,MAX_ROW] from start_col to end_col ([0,MAX_COL])
 * draw = 1 means draw line, draw = 0 means erase line
 */
signed int bt_hLine(unsigned int row, unsigned int start_col, unsigned int end_col, int1 draw) {
  return bt_hLine_display(STD_DISP, row, start_col, end_col, draw);
}

/**
 * inverts every pixel on the display
 */
signed int bt_invert() {
  return bt_invert_display(STD_DISP);
}


/**
 * rotates part of the display
 * //TODO check whether this works
 */
/*
   signed int bt_rotate(unsigned int start_row, unsigned int start_col, 
   unsigned int end_row, unsigned int end_col, int rotation) {
  return bt_rotate_display(STD_DISP, start_row, start_col, end_row, end_col, rotation);
   }
*/










/******************************************************************************/
/* DIRECT IMAGE BUFFER MANIPULATION                                           */
/******************************************************************************/

/**
 * row in [0,ROW_MAX], col in [0,COL_MAX]
 */
signed int iSetBit(unsigned int row, unsigned int col, int1 thebit) {
   //    return setBitPtr(imagebuffer, row, col, thebit);
   unsigned byte b;

   if (!inside(row, col)) return -1;

   b = imagebuffer[row / 8][col];

   if (thebit == 1)
      bit_set(b, row % 8);
   else
      bit_clear(b, row % 8);

   imagebuffer[row / 8][col] = b;
   return 0;
}

/**
 * row in [0,ROW_MAX], col in [0,COL_MAX]
 */
int1 iGetBit(unsigned int row, unsigned int col) {
   unsigned byte b;

   if (!inside(row, col)) return 0;

   b = imagebuffer[row / 8][col];
   return bit_test(b, row % 8);
}

/**
 * sets byte in image buffer
 * row = [0, MAX_ROW / 8], col = [0, MAX_COL]
 */
signed int iSetByte(unsigned int row, unsigned int col, byte thebyte) {
   //    return setBytePtr(imagebuffer, row, col, thebyte);
   if (!inside(row * 8, col)) return -1;

   imagebuffer[row][col] = thebyte;
   return 0;
}

/**
 * gets byte from image buffer
 * row = [0, MAX_ROW / 8], col = [0, MAX_COL]
 */
signed byte iGetByte(unsigned int row, unsigned int col) {
   //    return getBytePtr(imagebuffer, row, col);
   if (!inside(row * 8, col)) return -1;

   return imagebuffer[row][col];
}








/******************************************************************************/
/* IMAGE SAVING / LOADING                                                     */
/******************************************************************************/

/**
 * Stores the image currently written into the imagebuffer (by calls to
 * addToImage) into flash memory for later retrieval by loadImage.
 */
signed int bt_storeImage(unsigned int width, unsigned int height) {
   unsigned long lengthwritten, towrite, i;
   const unsigned long len = DISP_MEM;
   unsigned long count;
   //unsigned int start_cnt;
   unsigned byte writebuffer[FLASH_PAGE_SIZE];
   char text[17];

#ifdef DEBUG_DISPLAYS
   bt_clear_display(1, 0);
   sprintf(text, "storing image");
   bt_write_string_display(1, 0, 0, text);

   sprintf(text, "%lu", nextFlashPage);
   bt_write_string_display(1, 3, 0, text);
#endif

   lastSavedImageNumber++;
   if (lastSavedImageNumber > MAX_IMAGES) {
      sprintf(text, "TOO MANY IMAGES");
      bt_clear_display(1, 0);
      bt_write_string_display(1, 4, 0, text);
      return -3;
   }
   flashPageMap[lastSavedImageNumber] = nextFlashPage;

   //len = (long)width * (long)((height -1)/8 +1) +2;
   //writebuffer[0] = width;
   //writebuffer[1] = height;
   //start_cnt = 2;

   count = 0;
   lengthwritten = 0;
   while (lengthwritten < len) {
      towrite = FLASH_PAGE_SIZE;
      if (len - lengthwritten < FLASH_PAGE_SIZE) towrite = len - lengthwritten;
      // write part of the display data into flash
      // TODO check whether can directly write to writebuffer
      // copy to writebuffer
      //for (i = start_cnt; i < towrite; i++) {
      for (i = 0; i < towrite; i++) {
         writebuffer[i] = imagebuffer[0][count];
         count++;
      }
      //start_cnt = 0;

      while (!FlashWriteSequence(nextFlashPage, writebuffer, towrite)) {;}

      lengthwritten += towrite;
      nextFlashPage += towrite;
   }

#ifdef DEBUG_DISPLAYS
   sprintf(text, "image stored");
   bt_write_string_display(1, 1, 0, text);
   sprintf(text, "%lu", nextFlashPage);
   bt_write_string_display(1, 3, 6*6, text);
#endif
   return 0;
}


/**
 * Stores the image currently written into the imagebuffer (by calls to
 * addToImage) into flash memory for later retrieval by loadImage.
 * flashpos specifies the position in the flash memory
 * use numbers in the range of [4.752, 500.000]
 * currently, each image requires 480 bytes
 */
signed int bt_storeImageAt(int32 flashpos, unsigned int width, unsigned int height) {
   unsigned long lengthwritten, towrite, i;
   const unsigned long len = DISP_MEM;
   unsigned long count;
   //unsigned int start_cnt;
   unsigned byte writebuffer[FLASH_PAGE_SIZE];
#ifdef DEBUG_DISPLAYS
   char text[17];

   bt_clear_display(1, 0);
   sprintf(text, "storing image");
   bt_write_string_display(1, 0, 0, text);

   sprintf(text, "%lu", flashpos);
   bt_write_string_display(1, 0, 0, text);
#endif

   //len = (long)width * (long)((height -1)/8 +1) +2;
   //writebuffer[0] = width;
   //writebuffer[1] = height;
   //start_cnt = 2;

   count = 0;
   lengthwritten = 0;
   while (lengthwritten < len) {
      towrite = FLASH_PAGE_SIZE;
      if (len - lengthwritten < FLASH_PAGE_SIZE) towrite = len - lengthwritten;
      // write part of the display data into flash
      // TODO check whether can directly write to writebuffer
      // copy to writebuffer
      //for (i = start_cnt; i < towrite; i++) {
      for (i = 0; i < towrite; i++) {
         writebuffer[i] = imagebuffer[0][count];
         count++;
      }
      //start_cnt = 0;

      while (!FlashWriteSequence(flashpos, writebuffer, towrite)) {;}

      lengthwritten += towrite;
      flashpos += towrite;
   }

#ifdef DEBUG_DISPLAYS
   sprintf(text, "image stored");
   bt_write_string_display(1, 1, 0, text);
   sprintf(text, "%lu", flashpos);
   bt_write_string_display(1, 3, 6*6, text);
#endif
   return 0;
}



/**
 * Loads the imageNr'th image that has been saved
 */
signed int bt_loadImage(unsigned int imageNr) {
   int32 mempos;
   unsigned long lengthread, toread, i;
   unsigned long len = DISP_MEM;
   unsigned long count;
   ///unsigned int start_cnt, width, height;
      unsigned byte readbuffer[FLASH_PAGE_SIZE];
#ifdef DEBUG_DISPLAYS
      char text[17];
      bt_clear_display(1, 0);
#endif

      // if (imageNr > lastSavedImageNumber) return -2;
      mempos = flashPageMap[imageNr];

#ifdef DEBUG_DISPLAYS
      sprintf(text, "load %u at %lu", imageNr, mempos);
      bt_write_string_display(1, 0, 0, text);
#endif


      ///   while (!FlashReadSequence(mempos, readbuffer, 2)) {;}
         ///   // copy to imagebuffer
         ///   width = readbuffer[0];
         ///   height = readbuffer[1];
         ///   mempos += 2;
         ///   len = (long)width * (long)((height -1)/8 +1) +2;

         count = 0;
         lengthread = 0;
         while (lengthread < len) {
            toread = FLASH_PAGE_SIZE;
            if (len - lengthread < FLASH_PAGE_SIZE) toread = len - lengthread;
            // read part of the image data from flash
            while (!FlashReadSequence(mempos, readbuffer, toread)) {;}

            // copy to imagebuffer
            for (i = 0; i < toread; i++) {
               imagebuffer[0][count] = readbuffer[i];
               count++;
            }

            lengthread += toread;
            mempos += toread;
         }

#ifdef DEBUG_DISPLAYS
         sprintf(text, "loading done");
         bt_write_string_display(1, 3, 0, text);
#endif
         return 0;
}


/**
 * Loads the image found at flash memory position flashpos
 * flashpos specifies the start position of the image in the flash memory
 * use numbers in the range of [4.752, 500.000]
 * currently, each image requires 480 bytes
 */
signed int bt_loadImageAt(int32 flashpos) {
   unsigned long lengthread, toread, i;
   unsigned long len = DISP_MEM;
   unsigned long count;
   ///unsigned int start_cnt, width, height;
      unsigned byte readbuffer[FLASH_PAGE_SIZE];
#ifdef DEBUG_DISPLAYS
      char text[17];
      bt_clear_display(1, 0);

      sprintf(text, "load %lu", flashpos);
      bt_write_string_display(1, 0, 0, text);
#endif

      ///   while (!FlashReadSequence(mempos, readbuffer, 2)) {;}
         ///   // copy to imagebuffer
         ///   width = readbuffer[0];
         ///   height = readbuffer[1];
         ///   mempos += 2;
         ///   len = (long)width * (long)((height -1)/8 +1) +2;

         count = 0;
         lengthread = 0;
         while (lengthread < len) {
            toread = FLASH_PAGE_SIZE;
            if (len - lengthread < FLASH_PAGE_SIZE) toread = len - lengthread;
            // read part of the image data from flash
            while (!FlashReadSequence(flashpos, readbuffer, toread)) {;}

            // copy to imagebuffer
            for (i = 0; i < toread; i++) {
               imagebuffer[0][count] = readbuffer[i];
               count++;
            }

            lengthread += toread;
            flashpos += toread;
         }

#ifdef DEBUG_DISPLAYS
         sprintf(text, "loading done");
         bt_write_string_display(1, 3, 0, text);
#endif
         return 0;
}



/*
   signed int bt_storeImage(unsigned int width, unsigned int height) {
   unsigned long lengthwritten, towrite, i;
   unsigned long len, count;
   unsigned int start_cnt;
   unsigned byte writebuffer[FLASH_PAGE_SIZE];
   char text[17];

   #ifdef DEBUG_DISPLAYS
   bt_clear_display(1, 0);
   sprintf(text, "storing image");
   bt_write_string_display(1, 0, 0, text);

   sprintf(text, "%4li", nextFlashPage);
   bt_write_string_display(1, 3, 0, text);
   #endif

   lastSavedImageNumber++;
   if (lastSavedImageNumber > MAX_IMAGES) {
   sprintf(text, "TOO MANY IMAGES");
   bt_clear_display(1, 0);
   bt_write_string_display(1, 4, 0, text);
   return -3;
   }
   flashPageMap[lastSavedImageNumber] = nextFlashPage;

   len = (long)width * (long)((height -1)/8 +1) +2;
   writebuffer[0] = width;
   writebuffer[1] = height;
   start_cnt = 2;

   count = 0;
   lengthwritten = 0;
   while (lengthwritten < len) {
   towrite = FLASH_PAGE_SIZE;
   if (len - lengthwritten < FLASH_PAGE_SIZE) towrite = len - lengthwritten;
   // write part of the display data into flash
   // TODO check whether can directly write to writebuffer
   // copy to writebuffer
   for (i = start_cnt; i < towrite; i++) {
   writebuffer[i] = imagebuffer[0][count];
   count++;
   }
   start_cnt = 0;

   while (!FlashWriteSequence(nextFlashPage, writebuffer, towrite)) {;}

   lengthwritten += towrite;
   nextFlashPage += towrite;
   }

   #ifdef DEBUG_DISPLAYS
   sprintf(text, "image stored");
   bt_write_string_display(1, 1, 0, text);
   sprintf(text, "%4li", nextFlashPage-1);
   bt_write_string_display(1, 3, 6*6, text);
   #endif
   return 0;
   }


   signed int bt_loadImage(unsigned int imageNr) {
   int32 mempos;
   unsigned long lengthread, toread, i;
   unsigned long len, count;
   unsigned int start_cnt, width, height;
   unsigned byte readbuffer[FLASH_PAGE_SIZE];
   #ifdef DEBUG_DISPLAYS
   char text[17];
   bt_clear_display(1, 0);
   #endif

   // if (imageNr > lastSavedImageNumber) return -2;
   mempos = flashPageMap[imageNr];

   #ifdef DEBUG_DISPLAYS
   sprintf(text, "load %u at %li", imageNr, mempos);
   bt_write_string_display(1, 0, 0, text);
   #endif

   toread = 2;
   // read size of image from flash
   while (!FlashReadSequence(mempos, readbuffer, toread)) {;}
   width = readbuffer[0];
   height = readbuffer[1];
   mempos += 2;

   len = (long)width * (long)((height -1)/8 +1);

   #ifdef DEBUG_DISPLAYS
   sprintf(text, "w=%u,h=%u,l=%li", width, height, len);
   bt_write_string_display(1, 1, 1, text);
   #endif

   count = 0;
   lengthread = 0;
   while (lengthread < len) {
   toread = FLASH_PAGE_SIZE;
   if (len - lengthread < FLASH_PAGE_SIZE) toread = len - lengthread;
   // read part of the image data from flash
   while (!FlashReadSequence(mempos, readbuffer, toread)) {;}

   // copy to imagebuffer
   for (i = 0; i < toread; i++) {
   imagebuffer[0][count] = readbuffer[i];
   count++;
   }

   lengthread += toread;
   mempos += toread;
   }

   #ifdef DEBUG_DISPLAYS
   sprintf(text, "loading done");
   bt_write_string_display(1, 3, 0, text);
   #endif
   return 0;
   }

*/






/******************************************************************************/
/* IMAGE FUNCTIONS                                                            */
/******************************************************************************/

// --------------------------- USING THE IMAGE BUFFER ----------------------- //

/**
 * clears the image buffer
 */
signed int bt_resetImage() {
   unsigned int r, c;
   signed int ret = 0;
#ifdef DEBUG_DISPLAYS
   char text[17];
   sprintf(text, "resetting image");
   bt_write_string_display(1, 0, 0, text);
#endif

   // clear image buffer
   for (r = 0; r <= MAX_ROW / 8; r++)
      for (c = 0; c <= MAX_COL; c++)
         if (iSetByte(r, c, 0x00) < 0) ret = -1;

   // reset global image pointer
   image_x = 0;
   image_y = 0;
   /*
      PackInACL((unsigned int)image_x, ACL_TYPE_SGX_HI, ACL_TYPE_SGX_LO);
      PackInACL((unsigned int)image_y, ACL_TYPE_SGY_HI, ACL_TYPE_SGY_LO);
      while(ACLSendingBusy()){;}
      ACLSendPacket( 20 );
   */

#ifdef DEBUG_DISPLAYS
   sprintf(text, "image reset");
   bt_write_string_display(1, 1, 0, text);
#endif

   return ret;
}


/**
 * Adds data to the internal image buffer.
 * len specifies the amount of bytes in the given data
 * width and height specify the width and height of the whole image (not
 * only the part that is transmitted in this call)
 */
signed int bt_addToImage(unsigned char *data, unsigned long len, 
                         unsigned int width, unsigned int height) {
   unsigned long i;
   unsigned int c;

#ifdef DEBUG_DISPLAYS
   char text[17];
   sprintf(text, "adding %li %u %u", len, width, height);
   bt_write_string_display(1, BT_ROW_2, BT_CHAR_1, text);
#endif
   /*
      sprintf(text, "%u %u %u %u", data[0], data[1], data[2], data[3]);
      bt_write_string_display(0, BT_ROW_3, BT_CHAR_1, text);
   */

   for (i = 0; i < len; i++) {
      if (image_y + 7 > height) {
         // cannot write whole bytes
         /*
            PackInACL((unsigned int)image_x, ACL_TYPE_SGX_HI, ACL_TYPE_SGX_LO);
            PackInACL((unsigned int)image_y, ACL_TYPE_SGY_HI, ACL_TYPE_SGY_LO);
            PackInACL((unsigned int)data[i], ACL_TYPE_SGZ_HI, ACL_TYPE_SGZ_LO);
            while(ACLSendingBusy()){;}
            ACLSendPacket( 20 );
         */
         while (i < len && image_x < width) {
            /*
               sprintf(text, "%2u,%2u,%3u,%3li", image_y, image_x, data[i], i);
               bt_write_string_display(1, BT_ROW_1, BT_CHAR_1, text);
               sprintf(text, "(%2u,%3li)", width, len);
               bt_write_string_display(1, BT_ROW_2, BT_CHAR_1, text);
            */
            // write part of bytes that fit into height
            for (c = image_y; c < height; c++) {
               iSetBit(c, image_x, bit_test(data[i], c-image_y));
            }
            image_x++;
            i++;
         }

         // finished since otherwise the height would be exceeded
         return 0;

      } else {
         iSetByte(image_y / 8, image_x, data[i]);
         /*
            sprintf(text, "%2u,%2u,%3u,%3li", image_y / 8, image_x, data[i], i);
            bt_write_string_display(1, BT_ROW_3, BT_CHAR_1, text);
         */
         /*
            PackInACL((unsigned int)(image_y/8), ACL_TYPE_SGY_HI, ACL_TYPE_SGY_LO);
            PackInACL((unsigned int)image_x, ACL_TYPE_SGX_HI, ACL_TYPE_SGX_LO);
            PackInACL((unsigned int)data[i], ACL_TYPE_SGZ_HI, ACL_TYPE_SGZ_LO);
            while(ACLSendingBusy()){;}
            ACLSendPacket( 20 );
         */
         image_x++;
         if (image_x >= width) {
            image_x = 0;
            image_y += 8;
         }
         if (image_y > height) break;
      }
   }
   /*
      sprintf(text, "                ");
      bt_write_string_display(1, BT_ROW_1, BT_CHAR_1, text);
   */
}



// --------------------------- DISPLAY IMAGE METHODS ----------------------- //

/**
 * Write image in image buffer to standard display starting at (row, col) on 
 * the display where row in [0, MAX_ROW], col in [0, MAX_COL]
 * width and height specify the dimensions of the image to be written
 * rotation specified as DEGx
 */
signed int bt_writeImage(unsigned int row, unsigned int col, 
                         unsigned int width, unsigned int height, int rotation) {
   return bt_writeImage_display(STD_DISP, row, col, width, height, rotation);
}


/**
 * Write image in image buffer to the given display starting at (row, col) on 
 * the display where row in [0, MAX_ROW], col in [0, MAX_COL]
 * width and height specify the dimensions of the image to be written
 * rotation specified as DEGx
 */
signed int bt_writeImage_display(unsigned int dispnr, unsigned int row, unsigned int col, 
                                 unsigned int width, unsigned int height, int rotation) {
  unsigned int conv_row, conv_col;
  unsigned int r, c;
  signed int ret;

#ifdef DEBUG_DISPLAYS
  char text[17];
  sprintf(text, "writing image");
  bt_write_string_display(1, 0, 0, text);
#endif

  for (r = 0; r < height; r++) {
    for (c = 0; c < width; c++) {
      convert_coords(rotation, r, c, &conv_row, &conv_col, MAX_ROW, MAX_COL);

      if (rotation == DEG180) {
        conv_row -= MAX_ROW - height +1;
        conv_col -= MAX_COL - width +1;
      } else if (rotation == DEG90) {
        conv_row -= MAX_ROW - width +1;
      } else if (rotation == DEG270) {
        conv_col -= MAX_COL - height +1;
      }
      dsSetBit(dispnr, row+conv_row, col+conv_col, iGetBit(r, c));
    }
  }

  /*
  // send something (otherwise, the stack stops?!)
  PackInACL(width, ACL_TYPE_SGX_HI, ACL_TYPE_SGX_LO);
  PackInACL(height, ACL_TYPE_SGY_HI, ACL_TYPE_SGY_LO);
  while(ACLSendingBusy()){;}
  ACLSendPacket( 20 );
  */

  // TODO use more efficient method
  //i//ret = displayUpdate();
  ret = displayUpdate(dispnr);

#ifdef DEBUG_DISPLAYS
   sprintf(text, "image written");
   bt_write_string_display(1, 0, 0, text);
#endif

  return ret;
}


/**
 * like bt_writeImage but OR's the bits
 */
/*
   signed int bt_orImage_display(unsigned int dispnr, unsigned int row, unsigned int col, 
   unsigned int width, unsigned int height, int rotation) {
  // TODO
   }
*/



















#endif







// --------------------- attic


/*
  signed int bt_rotate(unsigned int start_row, unsigned int start_col, 
  unsigned int end_row, unsigned int end_col, int rotation, unsigned int new_row, unsigned int new_col) {

  unsigned int row, col, swapper;
  unsigned byte imageTmp[DISP_SIZE_X][DISP_SIZE_Y];

  // checks
  if (! (inside(start_row, start_col) && inside(end_row, end_col))) return -1;
  if (rotation != DEG0 && rotation != DEG90 && rotation != DEG180 && rotation != DEG270) return -2;

  // ensure start < end coordinates
  if (start_row > end_row) { swapper = start_row; start_row = end_row; end_row = swapper; }
  if (start_col > end_col) { swapper = start_col; start_col = end_col; end_col = swapper; }
   
  switch (rotation) {
  case DEG90:
  // first write rotated data to a temporary location
  for (row = new_row + end_col-start_col+1; row >= new_row; row--) {
  for (col = new_col + end_row-start_row+1; col >= new_col; col--) {
  // setBit(imageTmp, end_col - col + new_row, row - start_row  + new_col, getBit(imageTmp, row, col));
  setBit(imageTmp, end_col - col + new_row, row - start_row  + new_col,
  getBit(imageTmp, row, col));
  }
  }
         
  // then update the image buffer
  for (row = new_row + end_col-start_col+1; row >= new_row; row--) {
  for (col = new_col + end_row-start_row+1; col >= new_col; col--) {
  iSetBit(row, col, getBit(imageTmp, row, col));
  }
  }
      
  // and finally update the display
  for (row = (new_row + end_col-start_col+1) / 8; row >= new_row / 8; row--) {
  displayLineSegUpdate(row, new_col, new_col + end_row-start_row+1);
  }
  break;

  case DEG180:
  // first write rotated data to a temporary location
  for (row = new_row + end_col-start_col+1; row >= new_row; row--) {
  for (col = new_col + end_row-start_row+1; col >= new_col; col--) {
  // setBit(imageTmp, end_col - col + new_row, row - start_row  + new_col, getBit(imageTmp, row, col));
  setBit(imageTmp, end_col - col + new_row, row - start_row  + new_col,
  getBit(imageTmp, row, col));
  }
  }
         
  // then update the image buffer
  memcopy(imagebuffer, imageTmp, sizeof(imageTmp));
         
  //for (row = new_row + end_col-start_col+1; row >= new_row; row--) {
  //   for (col = new_col + end_row-start_row+1; col >= new_col; col--) {
  //      iSetBit(row, col, getBit(imageTmp, row, col));
  //   }
  //}
         
      
  // and finally update the display
  for (row = (new_row + end_col-start_col+1) / 8; row >= new_row / 8; row--) {
  displayLineSegUpdate(row, new_col, new_col + end_row-start_row+1);
  }
  break;

  case DEG270:
  // first write rotated data to a temporary location
  for (row = new_row + end_col-start_col+1; row >= new_row; row--) {
  for (col = new_col + end_row-start_row+1; col >= new_col; col--) {
  // setBit(imageTmp, end_col - col + new_row, row - start_row  + new_col, getBit(imageTmp, row, col));
  setBit(imageTmp, end_col - col + new_row, row - start_row  + new_col,
  getBit(imageTmp, row, col));
  }
  }
         
  // then update the image buffer
  for (row = new_row + end_col-start_col+1; row >= new_row; row--) {
  for (col = new_col + end_row-start_row+1; col >= new_col; col--) {
  iSetBit(row, col, getBit(imageTmp, row, col));
  }
  }
      
  // and finally update the display
  for (row = (new_row + end_col-start_col+1) / 8; row >= new_row / 8; row--) {
  displayLineSegUpdate(row, new_col, new_col + end_row-start_row+1);
  }
  break;
  }
   
  return 0;
  }
*/



/* ------------ do not work ---------------- */
/*
  signed int setBitPtr(unsigned byte* data, unsigned int row, unsigned int col, int1 thebit) {
  unsigned byte b;

  if (!inside(row, col)) return -1;

  ///b = data[row / 8][col];
  b = data[getIndex(row, col)];
  //b = *(data + row / 8 + col*MAX_COL);
  if (thebit == 1)
  bit_set(b, row % 8);
  else
  bit_clear(b, row % 8);
  ///data[row / 8][col] = b;
  data[getIndex(row, col)] = b;

  return 0;
  }

  int1 getBitPtr(unsigned byte *data, unsigned int row, unsigned int col) {
  unsigned byte b;

  if (!inside(row, col)) return 0;

  ///b = data[row / 8][col];
  b = data[getIndex(row, col)];
  return bit_test(b, row % 8);
  }

  signed int setBytePtr(unsigned byte *data, unsigned int row, unsigned int col, byte thebyte) {
  if (!inside(row, col)) return -1;

  data[getIndex(row, col)] = thebyte;
  return 0;
  }

  signed byte getBytePtr(unsigned byte *data, unsigned int row, unsigned int col) {
  if (!inside(row, col)) return -1;

  return data[getIndex(row, col)];
  }
*/






























/* ---------------------------------- old methods ----------------- */


/**
 * Writes the identity of each display on the display
 * \return nothing
 */
void write_identity_displays()
{
  int i = 0;
  char display_name[20];
  for (i = 0; i < 6; i++)
    {
      sprintf(display_name, "Display %i", i);
      bt_write_string_display(i,3,1,display_name);
    }    
}

/**
 * Clears all displays.
 * \return -1 on error, 0 else
 */
signed int bt_clear_displays() {
  signed int ret = 0;
  int i = 0;
  for (i = 0; i < 6; i++) {
    if (bt_clear_display(i, 0) < 0) ret = -1;
  }
  return ret;
}

/**
 * Writes a long int (length: 4 characters) to the standard display.
 * \param row the row to write to.
 * \param col the column to start writing to.
 * \param long_val the long integer to print.
 * \return nothing.
 */
void bt_write_long4(int row, int col, long long_val)
{
  char str_long[5];
  sprintf(str_long, "%04li", long_val);
  bt_write_string(row, col, str_long);
}   

/**
 * Writes a long int (length: 4 characters) to a named display.
 * \param display_no the display to write to.
 * \param row the row to write to.
 * \param col the column to start writing to.
 * \param long_val the long integer to print.
 */
void bt_write_long4_display(int display_no, int row, int col, long long_val)
{
  char str_long[5];
  sprintf(str_long, "%04li", long_val);
  bt_write_string_display(display_no, row, col, str_long);
}   
 
/**
 * Prints a character array to the standard display
 * \param row the row to write to.
 * \param col the column to start writing to.
 * \param str a pointer to a character array, terminated by <b>'\\0'</b>
 */
void bt_write_string(int row, int col, char * str)
{
  int strpos;
  strpos = 0;
   
  // change commented lines against for loop...
  for (; *str != '\0'; str++)
    //while (*str != '\0')
    {      
      bt_write_char(row, (col + (strpos * BT_CHARACTER_PIXEL_WIDTH)), *str);
      strpos++;
      //str++;
    }   
}

/**
 * Prints a character array to a named display
 * \param display_no the display to write to.
 * \param row the row to write to.
 * \param col the column to start writing to.
 * \param str a pointer to a character array, terminated by <b>'\\0'</b>
 */
void bt_write_string_display(int display_no, int row, int col, char * str)
{
  int strpos;
  strpos = 0;
   
  // change commented lines against for loop...
  for (; *str != '\0'; str++)
    //while (*str != '\0')
    {      
      bt_write_char_display(display_no, row, (col + (strpos * BT_CHARACTER_PIXEL_WIDTH)), *str);
      strpos++;
      //str++;
    }
}

/**
 * Writes the string "HCILAB.ORG" in <em>row</em> at character column 
 * <em>col</em>. Attention: Character position is <b>BT_CHARACTER_PIXEL_WIDTH</b>*col. 
 * So the starting position is defined in characters, not pixels.
 * \param display_no the display to write to.
 * \param row the row to write to.
 * \param col the column to start writing to.
 */
void bt_write_hcilab_display(int display_no, int row, int col)
{
  char hcilab[11] = "HCILAB.ORG";
  bt_write_string_display(display_no, row, col, hcilab);
  /*
    int cur_pos = 0;
    cur_pos = BT_CHARACTER_PIXEL_WIDTH * col;
    bt_write_char_display(display_no, row,cur_pos, 'H'); cur_pos=cur_pos+BT_CHARACTER_PIXEL_WIDTH;
    bt_write_char_display(display_no, row,cur_pos,'C'); cur_pos=cur_pos+BT_CHARACTER_PIXEL_WIDTH;
    bt_write_char_display(display_no, row,cur_pos,'I'); cur_pos=cur_pos+BT_CHARACTER_PIXEL_WIDTH;
    bt_write_char_display(display_no, row,cur_pos,'L'); cur_pos=cur_pos+BT_CHARACTER_PIXEL_WIDTH;
    bt_write_char_display(display_no, row,cur_pos,'A'); cur_pos=cur_pos+BT_CHARACTER_PIXEL_WIDTH;
    bt_write_char_display(display_no, row,cur_pos,'B'); cur_pos=cur_pos+BT_CHARACTER_PIXEL_WIDTH;
    bt_write_char_display(display_no, row,cur_pos,'.'); cur_pos=cur_pos+BT_CHARACTER_PIXEL_WIDTH;
    bt_write_char_display(display_no, row,cur_pos,'O'); cur_pos=cur_pos+BT_CHARACTER_PIXEL_WIDTH;
    bt_write_char_display(display_no, row,cur_pos,'R'); cur_pos=cur_pos+BT_CHARACTER_PIXEL_WIDTH;
    bt_write_char_display(display_no, row,cur_pos,'G');
  */
}

/**
 * Writes a line of the character ch to a named display.
 * \param display_no the display to write to.
 * \param row the row to write to.
 * \param ch the character to be written.
 */
void bt_write_line_display(int display_no, int row, int ch)
{
  int i;
  if (display_no == 0) {
#use i2c(master, sda=PIN_DISPLAY_1_I2C_SDA, scl=PIN_DISPLAY_1_I2C_SCL)

    i2c_start();
    i2c_write(0x7A);         // send address 0x7A
    i2c_write(0b01100000+row);
    i2c_write(0b00000000);
    for(i=0;i<100;i++){
      i2c_write(ch);
    }
    i2c_stop();      // end transfer
  }

  if (display_no == 1) {
#use i2c(master, sda=PIN_DISPLAY_2_I2C_SDA, scl=PIN_DISPLAY_2_I2C_SCL)
      
    i2c_start();
    i2c_write(0x7A);         // send address 0x7A
    i2c_write(0b01100000+row);
    i2c_write(0b00000000);
    for(i=0;i<100;i++){
      i2c_write(ch);
    }
    i2c_stop();      // end transfer
  }

  if (display_no == 2) {
#use i2c(master, sda=PIN_DISPLAY_3_I2C_SDA, scl=PIN_DISPLAY_3_I2C_SCL)
      
    i2c_start();
    i2c_write(0x7A);         // send address 0x7A
    i2c_write(0b01100000+row);
    i2c_write(0b00000000);
    for(i=0;i<100;i++){
      i2c_write(ch);
    }
    i2c_stop();      // end transfer
  }

  if (display_no == 3) {
#use i2c(master, sda=PIN_DISPLAY_4_I2C_SDA, scl=PIN_DISPLAY_4_I2C_SCL)
      
    i2c_start();
    i2c_write(0x7A);         // send address 0x7A
    i2c_write(0b01100000+row);
    i2c_write(0b00000000);
    for(i=0;i<100;i++){
      i2c_write(ch);
    }
    i2c_stop();      // end transfer
  }

  if (display_no == 4) {
#use i2c(master, sda=PIN_DISPLAY_5_I2C_SDA, scl=PIN_DISPLAY_5_I2C_SCL)
      
    i2c_start();
    i2c_write(0x7A);         // send address 0x7A
    i2c_write(0b01100000+row);
    i2c_write(0b00000000);
    for(i=0;i<100;i++){
      i2c_write(ch);
    }
    i2c_stop();      // end transfer
  }

  if (display_no == 5) {
#use i2c(master, sda=PIN_DISPLAY_6_I2C_SDA, scl=PIN_DISPLAY_6_I2C_SCL)
      
    i2c_start();
    i2c_write(0x7A);         // send address 0x7A
    i2c_write(0b01100000+row);
    i2c_write(0b00000000);
    for(i=0;i<100;i++){
      i2c_write(ch);
    }
    i2c_stop();      // end transfer
  }

}

/**
 * Writes a char to a named display.
 * \param display_no the display to write to.
 * \param row the row to write to.
 * \param col the column to start writing to.
 * \param ch the character to be written.
 */
// extract ascii from tables & write to LCD
void bt_write_char_display(int display_no, int row, int col, BYTE ch)   
{
  int tab_index;
  BYTE bt_char_bit_set;
  //int ch_pix_set;
  int i;
      
  if (ch<0x20)return;
  if (ch>0x7f)return;

  for (i=0;i<5;i++) {
    if (ch<0x50){
      tab_index=(((ch&0xff)-0x20)*5);
      bt_char_bit_set=table1[(tab_index+i)];
    } else if (ch>0x4f){
      tab_index=(((ch&0xff)-0x50)*5);
      tab_index=(((ch&0xff)-0x50)*5);
      tab_index=(((ch&0xff)-0x50)*5);
      bt_char_bit_set=table2[(tab_index+i)];
    }
    bt_write_pix_set_display(display_no, row, col+i, bt_char_bit_set,1);      
  }   
  bt_write_pix_set_display(display_no, row, col+5, 0x00,1);      
   
}

/**
 * ???
 * \param ch ???
 * \param count ???
 * \param display_no the display to write to.
 * \param row the row to write to.
 * \param col the column to start writing to.
 */
void bt_write_pix_set_display(int display_no, int row, int col, int ch, int count)
{
  int i;

  if (display_no == 0) {
#use i2c(master, sda=PIN_DISPLAY_1_I2C_SDA, scl=PIN_DISPLAY_1_I2C_SCL)

    i2c_start();
    i2c_write(0x7A);         // send address 0x7A
    i2c_write(0b01100000+row);
    i2c_write(0b00000000+col);
    for(i=0;i<count;i++){
      i2c_write(ch);
    }
    i2c_stop();      // end transfer
  }

  if (display_no == 1) {
#use i2c(master, sda=PIN_DISPLAY_2_I2C_SDA, scl=PIN_DISPLAY_2_I2C_SCL)
      
    i2c_start();
    i2c_write(0x7A);         // send address 0x7A
    i2c_write(0b01100000+row);
    i2c_write(0b00000000+col);
    for(i=0;i<count;i++){
      i2c_write(ch);
    }
    i2c_stop();      // end transfer
  }

  if (display_no == 2) {
#use i2c(master, sda=PIN_DISPLAY_3_I2C_SDA, scl=PIN_DISPLAY_3_I2C_SCL)
      

    i2c_start();
    i2c_write(0x7A);         // send address 0x7A
    i2c_write(0b01100000+row);
    i2c_write(0b00000000+col);
    for(i=0;i<count;i++){
      i2c_write(ch);
    }
    i2c_stop();      // end transfer
  }

  if (display_no == 3) {
#use i2c(master, sda=PIN_DISPLAY_4_I2C_SDA, scl=PIN_DISPLAY_4_I2C_SCL)
      

    i2c_start();
    i2c_write(0x7A);         // send address 0x7A
    i2c_write(0b01100000+row);
    i2c_write(0b00000000+col);
    for(i=0;i<count;i++){
      i2c_write(ch);
    }
    i2c_stop();      // end transfer
  }

  if (display_no == 4) {
#use i2c(master, sda=PIN_DISPLAY_5_I2C_SDA, scl=PIN_DISPLAY_5_I2C_SCL)
      

    i2c_start();
    i2c_write(0x7A);         // send address 0x7A
    i2c_write(0b01100000+row);
    i2c_write(0b00000000+col);
    for(i=0;i<count;i++){
      i2c_write(ch);
    }
    i2c_stop();      // end transfer
  }

  if (display_no == 5) {
#use i2c(master, sda=PIN_DISPLAY_6_I2C_SDA, scl=PIN_DISPLAY_6_I2C_SCL)
      

    i2c_start();
    i2c_write(0x7A);         // send address 0x7A
    i2c_write(0b01100000+row);
    i2c_write(0b00000000+col);
    for(i=0;i<count;i++){
      i2c_write(ch);
    }
    i2c_stop();      // end transfer
  }


}

/*
   void bt_clear_display(int display_no)
   {
  bt_write_line_display(display_no, 0x00,0x00);
  bt_write_line_display(display_no, 0x01,0x00);
  bt_write_line_display(display_no, 0x02,0x00);
  bt_write_line_display(display_no, 0x03,0x00);
  bt_write_line_display(display_no, 0x04,0x00);
   }
*/


/**
 * Writes a line of the character ch to the standard display. 
 * The line starts in <em>row</em>
 * and writes <em>ch</em> rows (binary representation of the
 * int (ch), 1 = row 1, 2 = row 2, 3 = row 1 & 2, 4 = row 3, ...)
 * \param row the row to write to.
 * \param ch the caracter to be written.
 */
void bt_write_line(int row, int ch)
{
  int i;
  i2c_start();
  // send address 0x7A
  i2c_write(0x7A);         
  i2c_write(0b01100000+row);
  i2c_write(0b00000000);
  for(i=0;i<100;i++)
    {
      i2c_write(ch);
    }
  // end transfer
  i2c_stop();      
}

/**
 * ???
 * \param count ???
 * \param ch ???
 * \param row the row to write to.
 * \param col the column to start writing to.
 */
void write_pix_set(int row, int col, int ch, int count)
{
  int i;
  i2c_start();
  // send address 0x7A
  i2c_write(0x7A);         
  i2c_write(0b01100000+row);
  i2c_write(0b00000000+col);
  for(i=0;i<count;i++)
    {
      i2c_write(ch);
    }
  // end transfer
  i2c_stop();      
}
/**
 * Deprecated. Use <em>bt_write_char</em> instead.
 * \param row the row to write to.
 * \param col the column to start writing to.
 * \param ch character to be written.
 */
void bt_print_char_lcd(int row, int col, BYTE ch)
{
  bt_write_char(row, col, ch);   
}

/**
 * Extracts ascii from tables & write to the standard display.
 * \param row the row to write to.
 * \param col the column to start writing to.
 * \param ch character to be written.
 */
void bt_write_char(int row, int col, BYTE ch)   
{
  int tab_index;
  BYTE bt_char_bit_set;
  //int ch_pix_set;
  int i;
      
  if (ch<0x20)return;
  if (ch>0x7f)return;

  for (i=0;i<5;i++) 
    {
      if (ch<0x50)
        {
          tab_index=(((ch&0xff)-0x20)*5);
          bt_char_bit_set=table1[(tab_index+i)];
        } 
      else if (ch>0x4f)
        {
          tab_index=(((ch&0xff)-0x50)*5);
          tab_index=(((ch&0xff)-0x50)*5);
          tab_index=(((ch&0xff)-0x50)*5);
          bt_char_bit_set=table2[(tab_index+i)];
        }
      write_pix_set(row, col+i, bt_char_bit_set,1);      
    }   
  write_pix_set(row, col+5, 0x00,1);         
}