
/* 
 * draws black dots on the image where there is
 * a feature. If the image is colour, then col
 * should be TRUE. If it is a grey image, then
 * it should be FALSE
 */

void draw_features(BYTE * image, int col)
{

  int i,j,k;
  
  for (k=0; k<XX*YY; k++)
  
	if (f[k].active == TRUE)
	  {
	    for (i=-1; i<=1; i++)
	      for (j=-1; j<=1; j++)
		{
		  if (col == FALSE)
		    image[ (f[k].y+j)*width + i+f[k].x ] = black;
		  else /* colour */
		    {
		      image[ 3*((f[k].y+j)*width + i+f[k].x) ]   = black;
		      image[ 3*((f[k].y+j)*width + i+f[k].x) +1] = black;
		      image[ 3*((f[k].y+j)*width + i+f[k].x) +2] = black;
		    }
		}

	    if (col == FALSE)
	      image[ (f[k].y*width) + f[k].x ] = white;
	    else
	      {
		image[ 3*((f[k].y*width) + f[k].x) ]   = white;
		image[ 3*((f[k].y*width) + f[k].x)+1 ] = white;
		image[ 3*((f[k].y*width) + f[k].x)+2 ] = white;
	      }
	  }
  

}



/*
 * Sets the image white and draws a black square in 
 * it
 */

void square_im(BYTE * imageIn, BYTE * imageOut)
{

  int i,j;

  memset(imageOut, white, width*height);
  
  for (i=20; i<35; i++)
    for (j=20; j<35; j++)
      imageOut[j*width+i] = black;

}



/*
 * Writes an image to a PGM file
 * careful of greyscale - 16 for grey16, 1 for 256
 */

void WritePGM(char *filename, unsigned char *pixel, 
	      int width, int height, int grey_scale)
{

  FILE        *fp = NULL;
  int          x, y;
  unsigned int ind;
  int scale;

  if (grey_scale == 16)
    scale = 16;
  else
    scale = 1;


  fp = fopen(filename,"wt");
  if (!fp)
    return;
  fprintf(fp,"P5\n");
  fprintf(fp,"%d %d\n",width,height);
  fprintf(fp,"255\n");
  for (y=0; y < height; y++) {
    for (x=0; x< width; x++) {
      ind = y*width+x;
      fprintf(fp,"%c",scale*pixel[ind]);
    }
  }

  fclose(fp);

}



/*
 * Draws a black grid on a grey scale image
 */

void draw_grid(window* w, BYTE * image, int col)
{

  int a,i,j;
  int x1,y1,x2,y2;


  for (i=0; i<XX; i++)
    for (j=0; j<YY; j++)
      {
	
	x1 = w[j*XX+i].x1;
	y1 = w[j*XX+i].y1;
	x2 = w[j*XX+i].x2;
	y2 = w[j*XX+i].y2;

   	for (a=x1; a<=x2; a++)
	  {
	    if (col == FALSE)
	      {
		image[y1*width+a] = black;
		image[y2*width+a] = black;
	      }
	    else
	      {
		image[ 3*(y1*width+a)   ] = black;
		image[ 3*(y1*width+a) +1] = black;
		image[ 3*(y1*width+a) +2] = black;
		image[ 3*(y2*width+a)   ] = black;
		image[ 3*(y2*width+a) +1] = black;
		image[ 3*(y2*width+a) +2] = black;
	      }
	  }
	for (a=y1; a<=y2; a++)
	  {
	    if (col == FALSE)
	      {
		image[a*width+x1] = black;
		image[a*width+x2] = black;
	      }
	    else
	      {
		image[ 3*(a*width+x1)   ] = black;
		image[ 3*(a*width+x1) +1] = black;
		image[ 3*(a*width+x1) +2] = black;
		image[ 3*(a*width+x2)   ] = black;
		image[ 3*(a*width+x2) +1] = black;
		image[ 3*(a*width+x2) +2] = black;
	      }
	  }
      }
}



/*
 * Draws points on a grey image 
 */

void draw_points(BYTE *image, IPLPoint *point, int no_points)
{

  int a,b,i;
   
  if (no_points != 0)

    for (i=0; i<no_points; i++)
      {
	for(a=-1; a<=1; a++)
	  for(b=-1; b<=1; b++)
	    image[ (point[i].y+b)*width + point[i].x+a ] = black;
	
	image[ (point[i].y*width) + point[i].x ] = 0x08;
      }

}


/*
 * Draws active area on a grey image
 */

void draw_active(BYTE *image, active* grid)
{

  int i;

  for (i=grid->main.x1; i<=grid->main.x2; i++)
    {
      image[grid->main.y1*width+i] = black;
      image[grid->main.y2*width+i] = black;
    }

  for (i=grid->main.y1; i<=grid->main.y2; i++)
    {
      image[i*width+grid->main.x1] = black;
      image[i*width+grid->main.x2] = black;
    }

}


/*
 * Draws the search area for a feature
 */

void  draw_areas(BYTE *image, int col)
{


  int i,k;

  int s = floor(SEARCH/2);
  
  int x1, y1, x2, y2;

  for (k=0; k<XX*YY; k++)
   
    if ( f[k].active == TRUE)
	{
	  x1 = f[k].x - s;
	  y1 = f[k].y - s;  
	  x2 = f[k].x + s;
	  y2 = f[k].y + s;
	  
	  if ( x1 < 0 )
	    x1 = 0;
  
	  if ( y1 < 0 )
	    y1 = 0;
	  
	  if ( x2 >=width )
	    x2 = width-1;
	  
	  if (y2 >= height )
	    y2 = height-1;
	  
	  for (i=x1; i<=x2; i++)
	    {

	      if (col == FALSE)
		{
		  image[ y1*width+i] = black;
		  image[ y2*width+i] = black;
		}
	      else /*colour*/
		{
		  image[ 3*(y1*width+i)   ] = black;
		  image[ 3*(y1*width+i) +1] = black;
		  image[ 3*(y1*width+i) +2] = black;

		  image[ 3*(y2*width+i)   ] = black;
		  image[ 3*(y2*width+i) +1] = black;
		  image[ 3*(y2*width+i) +2] = black;
		}
	    }
	  for (i=y1; i<=y2; i++)
	    {
	      
	      if (col == FALSE)
		{
		  image[ i*width+x1 ] = black;
		  image[ i*width+x2 ] = black;
		}
	      else
		{
		   image[ 3*(i*width+x1)   ] = black;
		   image[ 3*(i*width+x1) +1] = black;
		   image[ 3*(i*width+x1) +2] = black;
		   image[ 3*(i*width+x2)   ] = black;
		   image[ 3*(i*width+x2) +1] = black;
		   image[ 3*(i*width+x2) +2] = black;
		}

	    }
	  
	}
  
}

 
/*
 * Copies the array of features to another
 * gotta be a nicer way
 */
     
void copy_features()
{

  int k;

  for (k=0; k<XX*YY; k++)
    {
      
      if ( ( (motion == TRUE) && (f[k].moving == TRUE) ) 
	   || (motion == FALSE) )
	{
	  
	  g[k].x = f[k].x;
	  g[k].y = f[k].y;
	  g[k].grad_x = f[k].grad_x;
	  g[k].grad_y = f[k].grad_y;
	  g[k].active = f[k].active;
	  g[k].r = f[k].r;
	  g[k].g = f[k].g;
	  g[k].b = f[k].b;
	  g[k].moving = f[k].moving;
	}

    }

}


/*
 * Test image 1 for motion
 */

void motion_test_im1(BYTE* imageIn)
{

  int i,j;

  for (i=0; i<3*width*height; i++)
    {
      imageIn[3*i]   = white;
      imageIn[3*i+1] = white;
      imageIn[3*i+2] = white;
    }

  for (i=17; i<=32; i++)
    for (j=17; j<=32; j++)
      {
	imageIn[3*(j*width+i)]   = black;
	imageIn[3*(j*width+i)+1] = black;
	imageIn[3*(j*width+i)+2] = black;
      }
  
}


/*
 * Test image 2 for motion
 */
  
void motion_test_im2(BYTE* imageIn)
{

  int i,j;

  for (i=0; i<3*width*height; i++)
    {
      imageIn[3*i]   = white;
      imageIn[3*i+1] = white;
      imageIn[3*i+2] = white;
    }

  for (i=22; i<=37; i++)
    for (j=22; j<=37; j++)
      {
	imageIn[3*(j*width+i)]   = black;
	imageIn[3*(j*width+i)+1] = black;
	imageIn[3*(j*width+i)+2] = black;
      }
  
}
  
