File content as of revision 0:b560d4d15292:

#include "mbed.h"
#include "ADNS2051.h"
#include "MI0283QTlib.h"


/** Demo software for ADNS2051 optical sensor.
 * 
 */
 
// masks type
#define PREWITT             1
#define KIRSCH              2
#define SOBEL               3
// gamma value
#define GAMMA_25            1       // gamma value=2.5
#define GAMMA_22            2       // gamma value=2.2
#define GAMMA_18            3       // gamma value=1.8

/** Color and Edges enhancement
 * I took these two functions from the book: "Digital Media Processing DSP Algorithms Using C Hazarathaiah Malepati".
 * 
 * From the book:
 *   Histogram equalization technique of color enhancement in the RGB domain. 
 *   As we know, if the color component is represented with 8-bit precision, then the values 0 to 255 are used to represent a
 *   particular color component.We say that the image colors are well represented when the color components occupy
 *   the total range of 0 to 255. If the color components of an image do not occupy the full range (i.e., 0 to 255), then
 *   we have the scope to enhance the colors of that image.With the histogram equalization technique, first we find
 *   the minimum (Xmin) and maximum (Xmax) values of a particular color component, and then we translate that
 *   color component to have minimum value at zero by subtracting the Xmin from its values, and finally, multiply the
 *   color component by 255/(Xmax− Xmin) to obtain the maximum color component 255. This process is illustrated
 *   in Figure 10.1. The same process is applied for all three color components of an image. This process enhances
 *   image colors (green becomes greener, yellow becomes more yellow, etc.), 
 */
void color_enhancement( unsigned char *pixelmap);

/**
 * From the book:
 *   With edge enhancement, we increase the contrast of the image along the edges. The edge enhancement is done by
 *   strengthening the high-frequency components of an image. One way of enhancing edges is by adding weighted
 *   high-frequency components of an image to itself.
 */
void edges_enhancement( unsigned char *pixelmap);

/** From the book: Image processing in C Dwayne Phillips
 * The quick edge function performs edge detection using the single 3 x 3
 * quick mask. It performs convolution over the image array using the quick mask.
 * It thresholds the output image if requested, and xes the edges of the output
 * image.
 * Geometric operations change the spatial relationships between objects in an
 * image. They do this by moving objects around and changing the size and
 * shape of objects. Geometric operations help rearrange an image so we can
 * see what we want to see a little better.
 * The three basic geometric operations are displacement, stretching, and
 * rotation. A fourth operation is the cross product.
 * Displacement moves or displaces an image in the vertical and horizontal
 * directions. Stretching enlarges or reduces an image in the vertical and horizontal
 * directions. Rotation turns or rotates an image by any angle.
 */
void quick_edge(unsigned char *pixelmap, unsigned char *outmap, unsigned int threshold);
void setup_masks(unsigned int detect_type, int *mask_0, int *mask_1, int *mask_2, int *mask_3, int *mask_4, int *mask_5, int *mask_6, int *mask_7);
void perform_convolution(unsigned char *image, unsigned char *out_image, unsigned char detect_type, unsigned char threshold);
void geometry(unsigned char *the_image, unsigned char *out_image,
        float x_angle,
        float x_stretch, float y_stretch,
        int x_displace, int y_displace,
        float x_cross, float y_cross,
        int bilinear,
        int rows,
        int cols);
unsigned char bilinear_interpolate(unsigned char *the_image, double x, double y, int rows, int cols);
/* I found these books very useful and instructive. Recommend them wholeheartedly. */


/** Draw enlarged image to the x and y coordinates.
 * The image will be enlarged by 4.
 *
 * @param *pxlmap the array image
 * @param x the x position
 * @param y the y position
 */
void magnify( unsigned char *pxlmap, unsigned int x, unsigned int y);
void gammacorrection( unsigned char *pixelmap, unsigned char gammaval);

//
DigitalOut myled(LED2);
DigitalOut DBG_LED(LED1);
DigitalOut TS_CS(p15);
//
Serial pc(USBTX, USBRX);
//
ADNS2051 optical( p19, p20);
//
GLCD lcd(  p11, p12, p13, p14, p17, p26);

unsigned char pxlmap[256];
//unsigned char edgemap[256];
unsigned char tmpmap[256];

int new_hi=63, new_low=10;
int threshold_val=50;
float edges_val=0.5;

#define COLOR_LVL  32

/*
 #define COLOR_LVL  256
 
 gamma LUT generated using this formula:
 
 new_pixel=(( org_pixel/COLOR_LVL)^(1/gamma))*COLOR_LVL
 
    // set gamma value...
    gammaval=2.5;
    float f2=1/gammaval;
    // start LUT generation...
    for ( i=0; i<COLOR_LVL; i++) {
            float f1=(float)i/COLOR_LVL;
            gammaLUT[i] = pow( f1,f2)*COLOR_LVL;
    }
    
*/

/* LUT for gamma value: 2.5 */
unsigned char gmm25_LUT[32]={
    0, 7, 10, 12, 13, 15, 16, 17, 18, 19, 20, 20, 21, 22, 22, 23, 24,
    24, 25, 25, 26, 27, 27, 28, 28, 28, 29, 29, 30, 30, 31, 31, 
};

/* LUT for gamma value: 2.2 */
unsigned char gmm22_LUT[32]={
    0, 6, 9, 10, 12, 13, 14, 16, 17, 17, 18, 19, 20, 21, 21, 22, 23,
    24, 24, 25, 25, 26, 26, 27, 28, 28, 29, 29, 30, 30, 31, 31, 
};

/* LUT for gamma value: 1.8 */
unsigned char gmm18_LUT[32]={
    0, 4, 6, 8, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 21,
    22, 23, 23, 24, 25, 25, 26, 27, 27, 28, 29, 29, 30, 30, 31, 
};

void gammacorrection( unsigned char *pixelmap, unsigned char gammaval)
{
    unsigned char *pGammaLUT;
    unsigned char x0, y0;
    int M=16, N=16;
    int j, i, p, q;
    
    switch (gammaval) {
        case GAMMA_25:
            pGammaLUT=(unsigned char*)gmm25_LUT;
        break;
        case GAMMA_22:
            pGammaLUT=(unsigned char*)gmm22_LUT;
        break;
        case GAMMA_18:
            pGammaLUT=(unsigned char*)gmm18_LUT;
        break;
    }
    
    for(j = 0;j < M;j++){
        p = j*N;q = (j + 1)*N;
        for(i = p;i < q; i++){
            y0 = pixelmap[i];
            x0 = pGammaLUT[y0];
            pixelmap[i] = x0;
        }
    }
}

#define FILL    255

/*******************************************
*
*   geometry(..
*
*   This routine performs geometric
*   transformations on the pixels in an
*   image array.  It performs basic
*   displacement, stretching, and rotation.
*
*   The basic equations are:
*
*   new x = x.cos(a) + y.sin(a) + x_displace
*           + x.x_stretch +x.y.x_cross
*
*   new y = y.cos(a) - x.sin(a) + y_displace
*           + y.y_stretch +x.y.y_cross
*
*******************************************/
void geometry(unsigned char *the_image, unsigned char *out_image,
        float x_angle,
        float x_stretch, float y_stretch,
        int x_displace, int y_displace,
        float x_cross, float y_cross,
        int bilinear,
        int rows,
        int cols)
{
   double cosa, sina, radian_angle, tmpx, tmpy;
   float  fi, fj, x_div, y_div, x_num, y_num;
   int    i, j, new_i, new_j;


      /******************************
      *
      *   Load the terms array with
      *   the correct parameters.
      *
      *******************************/

      /* the following magic number is from
         180 degrees divided by pi */
   radian_angle = x_angle/57.29577951;
   cosa  = cos(radian_angle);
   sina  = sin(radian_angle);

      /************************************
      *
      *   NOTE: You divide by the
      *   stretching factors. Therefore, if
      *   they are zero, you divide by 1.
      *   You do this with the x_div y_div
      *   variables. You also need a
      *   numerator term to create a zero
      *   product.  You do this with the
      *   x_num and y_num variables.
      *
      *************************************/

   if(x_stretch < 0.00001){
      x_div = 1.0;
      x_num = 0.0;
   }
   else{
      x_div = x_stretch;
      x_num = 1.0;
   }

   if(y_stretch < 0.00001){
      y_div = 1.0;
      y_num = 0.0;
   }
   else{
      y_div = y_stretch;
      y_num = 1.0;
   }

      /**************************
      *
      *   Loop over image array
      *
      **************************/

   for(i=0; i<rows; i++){
      for(j=0; j<cols; j++){

         fi = i;
         fj = j;

         tmpx = (double)(j)*cosa         +
                (double)(i)*sina         +
                (double)(x_displace)     +
                (double)(x_num*fj/x_div) +
                (double)(x_cross*i*j);

         tmpy = (double)(i)*cosa         -
                (double)(j)*sina         +
                (double)(y_displace)     +
                (double)(y_num*fi/y_div) +
                (double)(y_cross*i*j);

         if(x_stretch != 0.0)
            tmpx = tmpx - (double)(fj*cosa + fi*sina);
         if(y_stretch != 0.0)
            tmpy = tmpy - (double)(fi*cosa - fj*sina);

         new_j = tmpx;
         new_i = tmpy;

         if(bilinear == 0){
            if(new_j < 0       ||
               new_j >= cols   ||
               new_i < 0       ||
               new_i >= rows)
               out_image[j+(i*rows)] = FILL;
            else
               out_image[j+(i*rows)] =
                the_image[new_j+(new_i*rows)];
         }  /* ends if bilinear */
         else{
            out_image[j+(i*rows)] = 
               bilinear_interpolate(the_image,
                                    tmpx, tmpy,
                                    rows, cols);
         }  /* ends bilinear if */

      }  /* ends loop over j */
   }  /* ends loop over i */

}  /* ends geometry */



/*******************************************
*
*   bilinear_interpolate(..
*
*   This routine performs bi-linear
*   interpolation.
*
*   If x or y is out of range, i.e. less
*   than zero or greater than rows or cols,
*   this routine returns a zero.
*
*   If x and y are both in range, this
*   routine interpolates in the horizontal
*   and vertical directions and returns
*   the proper gray level.
*
*******************************************/
unsigned char bilinear_interpolate(unsigned char *the_image, double x, double y, int rows, int cols)
{
   double fraction_x, fraction_y,
          one_minus_x, one_minus_y,
          tmp_double;
   int    ceil_x, ceil_y, floor_x, floor_y;
   short  p1, p2, p3, result = FILL;

      /******************************
      *
      *   If x or y is out of range,
      *   return a FILL.
      *
      *******************************/

   if(x < 0.0               ||
      x >= (double)(cols-1)   ||
      y < 0.0               ||
      y >= (double)(rows-1))
      return(result);

   tmp_double = floor(x);
   floor_x    = tmp_double;
   tmp_double = floor(y);
   floor_y    = tmp_double;
   tmp_double = ceil(x);
   ceil_x     = tmp_double;
   tmp_double = ceil(y);
   ceil_y     = tmp_double;

   fraction_x = x - floor(x);
   fraction_y = y - floor(y);

   one_minus_x = 1.0 - fraction_x;
   one_minus_y = 1.0 - fraction_y;

   tmp_double = one_minus_x * 
          (double)(the_image[floor_x+(floor_y*rows)]) +
          fraction_x * 
          (double)(the_image[ceil_x+(floor_y*rows)]);
   p1         = tmp_double;

   tmp_double = one_minus_x * 
          (double)(the_image[floor_x+(ceil_y*rows)]) +
          fraction_x * 
          (double)(the_image[ceil_x+(ceil_y*rows)]);
   p2         = tmp_double;

   tmp_double = one_minus_y * (double)(p1) +
          fraction_y * (double)(p2);
   p3         = tmp_double;


   return(p3);

}  /* ends bilinear_interpolate */



/***************************
*
*   Directions for the masks
*  3 2 1
*  4 x 0
*  5 6 7
*
****************************/
/* masks for kirsch operator */
int kirsch_mask_0[9] =  {
        5,  5,  5,
       -3,  0, -3,
       -3, -3, -3};

int kirsch_mask_1[9] =  {
       -3,  5,  5,
       -3,  0,  5,
       -3, -3, -3};

int kirsch_mask_2[9] =  {
       -3, -3,  5,
       -3,  0,  5,
       -3, -3,  5};

int kirsch_mask_3[9] =  {
       -3, -3, -3,
       -3,  0,  5,
       -3,  5,  5};

int kirsch_mask_4[9] =  {
       -3, -3, -3,
       -3,  0, -3,
        5,  5,  5};

int kirsch_mask_5[9] =  {
       -3, -3, -3,
        5,  0, -3,
        5,  5, -3};

int kirsch_mask_6[9] =  {
        5, -3, -3,
        5,  0, -3,
        5, -3, -3};

int kirsch_mask_7[9] =  {
        5,  5, -3,
        5,  0, -3,
       -3, -3, -3};


   /* masks for prewitt operator */

int prewitt_mask_0[9] =  {
        1,  1,  1,
        1, -2,  1
       -1, -1, -1 };

int prewitt_mask_1[9] =  {
        1,  1,  1,
        1, -2, -1,
        1, -1, -1 };

int prewitt_mask_2[9] =  {
        1,  1, -1,
        1, -2, -1,
        1,  1, -1 };

int prewitt_mask_3[9] =  {
        1, -1, -1,
        1, -2, -1,
        1,  1,  1};

int prewitt_mask_4[9] =  {
       -1, -1, -1,
        1, -2,  1,
        1,  1,  1};

int prewitt_mask_5[9] =  {
       -1, -1,  1,
       -1, -2,  1,
        1,  1,  1};

int prewitt_mask_6[9] =  {
       -1,  1,  1,
       -1, -2,  1,
       -1,  1,  1};

int prewitt_mask_7[9] =  {
        1,  1,  1,
       -1, -2,  1,
       -1, -1,  1};


   /* masks for sobel operator */

int sobel_mask_0[9] =  {
        1,  2,  1,
        0,  0,  0,
       -1, -2, -1};

int sobel_mask_1[9] =  {
        2,  1,  0,
        1,  0, -1,
        0, -1, -2 };

int sobel_mask_2[9] =  {
        1,  0, -1,
        2,  0, -2,
        1,  0, -1};
int sobel_mask_3[9] =  {
        0, -1, -2,
        1,  0, -1,
        2,  1,  0 };
int sobel_mask_4[9] =  {
       -1, -2, -1,
        0,  0,  0,
        1,  2,  1};

int sobel_mask_5[9] =  {
       -2, -1,  0,
       -1,  0,  1,
        0,  1,  2};

int sobel_mask_6[9] =  {
       -1,  0,  1,
       -2,  0,  2,
       -1,  0,  1};

int sobel_mask_7[9] =  {
        0,  1,  2,
       -1,  0,  1,
       -2, -1,  0};


/***********************************************
*
*    setup_masks(...
*
*    This function copies the mask values defined
*    at the top of this file into the mask
*    arrays mask_0 through mask_7.
*
***********************************************/
void setup_masks(unsigned int detect_type, int *mask_0, int *mask_1, int *mask_2, int *mask_3, int *mask_4, int *mask_5, int *mask_6, int *mask_7)
{
   int i;

   if(detect_type == KIRSCH){
    for(i=0; i<9; i++){
        mask_0[i] = kirsch_mask_0[i];
        mask_1[i] = kirsch_mask_1[i];
        mask_2[i] = kirsch_mask_2[i];
        mask_3[i] = kirsch_mask_3[i];
        mask_4[i] = kirsch_mask_4[i];
        mask_5[i] = kirsch_mask_5[i];
        mask_6[i] = kirsch_mask_6[i];
        mask_7[i] = kirsch_mask_7[i];
    }
   }  /* ends if detect_type == KIRSCH */


   if(detect_type == PREWITT){
      for(i=0; i<9; i++){
          mask_0[i] = prewitt_mask_0[i];
          mask_1[i] = prewitt_mask_1[i];
          mask_2[i] = prewitt_mask_2[i];
          mask_3[i] = prewitt_mask_3[i];
          mask_4[i] = prewitt_mask_4[i];
          mask_5[i] = prewitt_mask_5[i];
          mask_6[i] = prewitt_mask_6[i];
          mask_7[i] = prewitt_mask_7[i];
      }
   }  /* ends if detect_type == PREWITT */


   if(detect_type == SOBEL){
      for(i=0; i<9; i++){
          mask_0[i] = sobel_mask_0[i];
          mask_1[i] = sobel_mask_1[i];
          mask_2[i] = sobel_mask_2[i];
          mask_3[i] = sobel_mask_3[i];
          mask_4[i] = sobel_mask_4[i];
          mask_5[i] = sobel_mask_5[i];
          mask_6[i] = sobel_mask_6[i];
          mask_7[i] = sobel_mask_7[i];
      }
   }  /* ends if detect_type == SOBEL */

}  /* ends setup_masks */

/**********************************************************
*
*   perform_convolution(...
*
*   This function performs convolution between the input
*   image and 8 3x3 masks.  The result is placed in
*   the out_image.
*
********************************************************/
void perform_convolution(unsigned char *image, unsigned char *out_image, unsigned char detect_type, unsigned char threshold)
{

    int b, i, j, sum, p, q, c, k;

    int mask_0[9];
    int mask_1[9];
    int mask_2[9];
    int mask_3[9];
    int mask_4[9];
    int mask_5[9];
    int mask_6[9];
    int mask_7[9];

    int max,
        min,
        new_hi,
        new_low;

    int M=16, N=16;

    setup_masks(detect_type, &mask_0[0], &mask_1[0],&mask_2[0], &mask_3[0], &mask_4[0], &mask_5[0],&mask_6[0], &mask_7[0]);

    new_hi  = 60;
    new_low = 10;

    min = 10;
    max = 63;

    /* clear output image array */
    for(i=0; i<N*M; i++)
        out_image[i] = 0;

    for( j = 1;j < (M-1);j++){
        p = j*N; q = (j+1)*N;
        for( i = (p+1);i < (q-1);i++) {

         /* Convolve for all 8 directions */

         /* 0 direction */

        sum = 0;
        c=0;
        for( k=(i-N); k<=(i+N); k+=N) {
            for(b=-1; b<2; b++){
               sum = sum + image[k+b] * mask_0[c++];
            }
        }
        if(sum < 0)   sum = 0;
        if(sum > max) sum = max;
        if(sum > out_image[i])
            out_image[i] = sum;


         /* 1 direction */

        sum = 0;
        c=0;
        for( k=(i-N); k<=(i+N); k+=N) {
            for(b=-1; b<2; b++){
               sum = sum + image[k+b] * mask_1[c++];
            }
        }
        if(sum < 0)   sum = 0;
        if(sum > max) sum = max;
        if(sum > out_image[i])
            out_image[i] = sum;


         /* 2 direction */

        sum = 0;
        c=0;
        for( k=(i-N); k<=(i+N); k+=N) {
            for(b=-1; b<2; b++){
               sum = sum + image[k+b] * mask_2[c++];
            }
        }
        if(sum < 0)   sum = 0;
        if(sum > max) sum = max;
        if(sum > out_image[i])
            out_image[i] = sum;


         /* 3 direction */

        sum = 0;
        c=0;
        for( k=(i-N); k<=(i+N); k+=N) {
            for(b=-1; b<2; b++){
               sum = sum + image[k+b] * mask_3[c++];
            }
        }
        if(sum < 0)   sum = 0;
        if(sum > max) sum = max;
        if(sum > out_image[i])
            out_image[i] = sum;


         /* 4 direction */

        sum = 0;
        c=0;
        for( k=(i-N); k<=(i+N); k+=N) {
            for(b=-1; b<2; b++){
               sum = sum + image[k+b] * mask_4[c++];
            }
        }
        if(sum < 0)   sum = 0;
        if(sum > max) sum = max;
        if(sum > out_image[i])
            out_image[i] = sum;


         /* 5 direction */

        sum = 0;
        c=0;
        for( k=(i-N); k<=(i+N); k+=N) {
            for(b=-1; b<2; b++){
               sum = sum + image[k+b] * mask_5[c++];
            }
        }
        if(sum < 0)   sum = 0;
        if(sum > max) sum = max;
        if(sum > out_image[i])
            out_image[i] = sum;


         /* 6 direction */

        sum = 0;
        c=0;
        for( k=(i-N); k<=(i+N); k+=N) {
            for(b=-1; b<2; b++){
               sum = sum + image[k+b] * mask_6[c++];
            }
        }
        if(sum < 0)   sum = 0;
        if(sum > max) sum = max;
        if(sum > out_image[i])
            out_image[i] = sum;


         /* 7 direction */

        sum = 0;
        c=0;
        for( k=(i-N); k<=(i+N); k+=N) {
            for(b=-1; b<2; b++){
               sum = sum + image[k+b] * mask_7[c++];
            }
        }
        if(sum < 0)   sum = 0;
        if(sum > max) sum = max;
        if(sum > out_image[i])
            out_image[i] = sum;


      }  /* ends loop over j */
   }  /* ends loop over i */

   /* if desired, threshold the output image */
   if(threshold == 1){
    for( j = 0;j < M;j++){
        p = j*N; q = (j+1)*N;
        for( i = p;i < q;i++) {
             if(out_image[i] > 50){
                  out_image[i] = new_hi;
             }
             else{
                  out_image[i] = new_low;
             }
          }
       }
   }  /* ends if threshold == 1 */

}  /* ends perform_convolution */


/*******************************************
 *
 *   quick_edge(...
 *
 *   This function finds edges by using
 *   a single 3x3 mask.
 *
 *******************************************/
void quick_edge(unsigned char *pixelmap, unsigned char *outmap, unsigned int threshold)
{
    int  i, j, k, c, p, q, b;
    int max, sum;

    int M=16, N=16;

    int quick_mask[9] =  {
       -1,  0, -1,
        0,  4,  0,
       -1,  0, -1 };

    max = 63;

    /* Do convolution over image array */
    for( j = 1;j < (M-1);j++){
        p = j*N; q = (j+1)*N;
        for( i = (p+1);i < (q-1);i++) {
            sum = 0;
            c=0;
            for( k=(i-N); k<=(i+N); k+=N) {
                for(b=-1; b<2; b++){
                   sum = sum + pixelmap[k+b] * quick_mask[c++];
                }
             }
            if(sum < 0)   sum = 0;
            if(sum > max) sum = max;
            outmap[i] = sum;
        }  /* ends loop over i */
    }  /* ends loop over j */

   /* if desired, threshold the output image */
   if(threshold == 1){
    for( j = 0;j < M;j++){
        p = j*N; q = (j+1)*N;
        for( i = p;i < q;i++) {
             if(outmap[i] > threshold_val){
                  outmap[i] = new_hi;
             }
             else{
                  outmap[i] = new_low;
             }
          }
       }
   }  /* ends if threshold == 1 */

}  /* ends quick_edge */



void edges_enhancement( unsigned char *pixelmap)
{
    int i, j, N, M, p, q;
    unsigned char BufX[256];
    int y0, x0;
    
    N=16;
    M=16;

    for(j = 0;j < 1;j++) { // copy top row
        p = j*N; q = (j+1)*N;
        for(i = p;i < q;i++)
            BufX[i] = pixelmap[i];
    }
    for(j = 0;j < M;j++) { // copy left column
        p = j*N;
        BufX[p] = pixelmap[p];
    }
    for(j = 1;j < M-1;j++) {
        p = j*N+1; q = (j+1)*N-1;
        for(i = p;i < q;i++) {
            x0 = pixelmap[i]; y0 = pixelmap[i-1];
            x0 = x0 << 2;
            x0 = x0 - y0; y0 = pixelmap[i+1];
            x0 = x0 - y0; y0 = pixelmap[i-N-1];
            x0 = x0 - y0; y0 = pixelmap[i+N+1];
            x0 = x0 - y0;
            x0 = (int) pixelmap[i] + (int) x0*edges_val;
            if (x0 < 0) x0 = 0;
            if (x0 > 63) x0 = 63;
            BufX[i] = (unsigned char) x0;
        }
    }
    //
    for(j = 0;j < M;j++){
        p = j*N; q = (j+1)*N;
        for(i = p;i < q;i++)
            pixelmap[i] = BufX[i];
    }
}

void color_enhancement( unsigned char *pixelmap)
{
    int i, j, N, M, p, q;
    unsigned char Hist[64];
    int y0, x0;
    float x;
    
    N=16;
    M=16;
    
    for(i=0; i<64; i++)
        Hist[i]=0;

    // Starting the color enhancement
    //
    for(j = 0;j < M;j++){
        p = j*N;
        q = (j+1)*N;
        for(i = p;i < q;i++)
        Hist[ pixelmap[i]] += 1;
    }

    y0 = 0;
    for(p = 0;p < 64;p++) {
        y0+= Hist[p];
        if (y0 > 32)
            break;
    }

    y0=p;
    x = 0;
    for(j = 0;j < M;j++) {
        p = j*N; q = (j+1)*N;
        for(i = p;i < q;i++) {
            x0 = (int) pixelmap[i] - y0;
            if (x0 < 0)
                x0 = 0;
            if (x0 > 63)
                x0 = 63;
            pixelmap[i] = x0;
        }
    }

    for(j = 63;j > 0;j--)
        if (Hist[j] > 0) break;

    x = (float) 64.0/(64-y0 + 63-j);

    for(j = 0;j < M;j++) {
        p = j*N; q = (j+1)*N;
        for(i = p;i < q;i++)
            pixelmap[i] = (unsigned char) ((float) pixelmap[i]*x + 0.5);
    }

}

int main() {

    char buf[256], c;
    int i, ii;
    int thrshld=0;
    int masktype=1;
    int gammaval=0;     // gamma OFF
        
    myled = 0;
    TS_CS=1;
    //
    int subidx, idx;
    
    lcd.lcd_init();
    lcd.lcd_clear( LCD_WHITE);
    lcd.backlightset( 1);
    lcd.lcd_drawstr( "Images from optical mouse: Init", 0, 0, lcd.lcd_RGB( 0, 255, 0));
    
    while( optical.init() );
    optical.setframerate();
    
    sprintf(buf, "Images from optical mouse: PixelDump [Chip rev: 0x%X]", optical.get_revisionID());
    lcd.lcd_drawstr( buf, 0, 0, lcd.lcd_RGB( 0, 255, 0));
    wait( 1.0);
    
    //
    while( 1) {
    
        // you can use the serial terminal to change some parameters:
        // [T/t] threshold ON/OFF
        // [V/v] threshold value inc/dec 
        // [H/h] high value inc/dec
        // [E/e] edge enhancement inc/dec
        // [m]   change mask type
        // [g]   change gamma value
        //
        if( pc.readable()) {
            c = pc.getc();
            if ( c=='t') {
                thrshld=0;
                printf("thrshld OFF\r\n");
            }
            if ( c=='T') {
                thrshld=1;
                printf("thrshld ON\r\n");
            }
            // threshold_val, new_hi, new_low
            if ( c=='V') {
                threshold_val++;
                if ( threshold_val>63)
                    threshold_val=63;
                printf("threshold_val=%d\r\n", threshold_val);
            }
            if ( c=='v') {
                threshold_val--;
                if ( threshold_val<0)
                    threshold_val=0;
                printf("threshold_val=%d\r\n", threshold_val);
            }
            if ( c=='H') {
                new_hi++;
                if ( new_hi>63)
                    new_hi=63;
                printf("new_hi=%d\r\n", new_hi);
            }
            if ( c=='h') {
                new_hi--;
                if ( new_hi<0)
                    new_hi=0;
                printf("new_hi=%d\r\n", new_hi);
            }
            /* edges enhancement */
            if ( c=='e') {
                edges_val-=0.1;
                if ( edges_val<0)
                    edges_val=0;
                printf("edges_val=%1.2f\r\n", edges_val);
            }
            if ( c=='E') {
                edges_val+=0.1;
                if ( edges_val>1)
                    edges_val=1;
                printf("edges_val=%1.2f\r\n", edges_val);
            }                        
            // 
            if ( c=='m') {
                masktype+=1;
                if ( masktype>3)
                    masktype=1;
                printf("masktype=%d\r\n", masktype);
            }            
            // 
            if ( c=='g') {
                gammaval+=1;
                if ( gammaval>3)
                    gammaval=0;
                printf("gammaval=%d\r\n", gammaval);
            }                        
        }
        
        unsigned char mv=optical.readmotion();
        sprintf(buf, "SQUAL:%d, dX:%+d, dY:%+d, Motion:0x%X    ", optical.surfacequality(), optical.deltaX, optical.deltaY, mv);
        lcd.lcd_drawstr( buf, 0, 200, lcd.lcd_RGB( 0, 255, 0));
        
        sprintf(buf, "AvrPxl:%d, MxPxl:%d    ", optical.averagepixel(), optical.maxpixelval() );
        lcd.lcd_drawstr( buf, 0, 220, lcd.lcd_RGB( 0, 255, 0));
        
        optical.pixeldump( &tmpmap[0]);

        if ( gammaval) {
            gammacorrection( &tmpmap[0], gammaval);
        }
        
        // pixelmap has the correct order: pos 0 is the left,upper image position.
        // Refer to the DS pag:35 about how to display the array
        c=0;
        subidx=0xFF;
        for( i=0;i<16;i++) {
            idx=subidx;
            for ( ii=0; ii<16; ii++) {
                pxlmap[c++]=tmpmap[idx];
                idx-=16;
            }
            subidx--;
        }
        
        // Use the feature geometry only for implementing the bilinear better viewing.
        geometry( &pxlmap[0], &tmpmap[0], 
                0.0,                        // angle
                0.0, 0.0,                   // streatch x, y
                0.0, 0.0,                   // displace x, y
                0.0, 0.0,                   // cross x, y
                1,                          // bilinear ON/OFF
                16, 16);                    // rows, cols
        
        // I copy the correct image in the array pxlmap to use it as a basis for successive processing.
        for ( i=0; i<(16*16); i++)
            pxlmap[i] = tmpmap[i];
     
        // 1 
        magnify( &tmpmap[0], 20, 20);

        //
        for( i=0;i<256;i++) {
            tmpmap[i]=pxlmap[i];
        }
        
        // 2
        color_enhancement( &tmpmap[0]);
        magnify( &tmpmap[0], 120, 20);
        
        //
        for( i=0;i<256;i++) {
            tmpmap[i]=pxlmap[i];
        }
        
        // 3
        edges_enhancement( &tmpmap[0]);
        magnify( &tmpmap[0], 220, 20);
        
        // 4
        quick_edge( &pxlmap[0], &tmpmap[0], thrshld);
        magnify( &tmpmap[0], 20, 100);
        
        // 5
        perform_convolution( &pxlmap[0], &tmpmap[0], masktype, thrshld);
        magnify( &tmpmap[0], 120, 100);

        // 6
        // visualizzo l'immagine originale...        
        magnify( &pxlmap[0], 220, 100);

    }
    
    while(1) {
        myled = 1;
        wait(0.2);
        myled = 0;
        wait(0.2);
    }
}

/* use the geometry function to magnify the image. */
void magnify( unsigned char *pxlmap, unsigned int x, unsigned int y)
{
    unsigned int r, c;
    float xy_str=(float)64/16;
    unsigned char tmpmap[256];
    
    // inizio ciclo...   
    for ( r=0; r<16; r+=4) {
        for ( c=0; c<16; c+=4) {
            // eseguo lo zoom sull'immagine...
            geometry( &pxlmap[0], &tmpmap[0], 
                    0.0,                        // angle
                    xy_str, xy_str,             // streatch x, y
                    (float)c, (float)r,         // displace x, y
                    0.0, 0.0,                   // cross x, y
                    1,                          // bilinear ON/OFF
                    16, 16);                    // rows, cols
            // la visualizzo...        
            int idx=0;
            for( int i=0;i<16;i++) {
                for ( int ii=0; ii<16; ii++) {
                    lcd.lcd_drawpixel( x+((c/4)*16)+ii, y+((r/4)*16)+i, lcd.lcd_RGB( tmpmap[idx],tmpmap[idx],tmpmap[idx]) );
                    idx++;
                }
            }
        }
    }
}