

/*
 * From the detected features, calculate the point (in
 * the image) where the camera should be directed 
 */

void direct(IPLPoint* target)
{

  int i;

  double no_feats=0;
  double x = 0;
  double y = 0;
  
  for (i=0; i<XX*YY; i++)
    if (f[i].active == TRUE)
      no_feats++;

  for (i=0; i<XX*YY; i++)
    if (f[i].active == TRUE)
	{
	  x += f[i].x;
	  y += f[i].y;
	}
   
  target->x = (int)(x/no_feats);
  target->y = (int)(y/no_feats);

}


/*
 * Draw crosshair on image to show where target is
 */

void draw_crosshair(BYTE* image, IPLPoint *target, int col)
{

  int i;
  
  int x = target->x;
  int y = target->y;

  /* clamp if too close to edge */

  if (x < 5)
    x = 5;
  
  if (x >= width-5)
    x = width-6;

  if (y < 5)
    y = 5;

  if (y >= height-5)
    y = height-6;


  if (col == FALSE) /* grey image */
    {

      for (i=-3; i<=3; i++)
	{
	  image[ (y-3)*width+x+i ] = black;
	  image[ (y+3)*width+x+i ] = black;
	}
      
      for (i=-3; i<=3; i++)
	{
	  image[ (y+i)*width+x-3 ] = black;
	  image[ (y+i)*width+x+3 ] = black;
	}

      for (i=-5; i<=5; i++)
	{
	  image[ y*width+x+i  ] = white;
	  image[ (y+i)*width+x] = white;
	}

    }

  else /* col image */
    {

      for (i=-3; i<=3; i++)
	{
	  image[ 3*((y-3)*width+x+i) ] = black;
	  image[ 3*((y+3)*width+x+i) ] = black;
	  
	  image[ 3*((y-3)*width+x+i)+1 ] = black;
	  image[ 3*((y+3)*width+x+i)+1 ] = black;

	  image[ 3*((y-3)*width+x+i)+2 ] = black;
	  image[ 3*((y+3)*width+x+i)+2 ] = black;
	}

      for (i=-3; i<=3; i++)
	{
	  image[ 3*((y+i)*width+x+3) ]   = black;
	  image[ 3*((y+i)*width+x-3) ]   = black;
	  
	  image[ 3*((y+i)*width+x+3)+1 ] = black;
	  image[ 3*((y+i)*width+x-3)+1 ] = black;

	  image[ 3*((y+i)*width+x+3)+2 ] = black;
	  image[ 3*((y+i)*width+x-3)+2 ] = black;
	}

      for (i=-5; i<=5; i++)
	{
	  image[ 3*( (y+i)*width+x ) ]   = white;
	  image[ 3*( (y+i)*width+x )+1 ] = black;
	  image[ 3*( (y+i)*width+x )+2 ] = black;
	  
	  image[ 3*( y*width+x+i ) ]   = white;
	  image[ 3*( y*width+x+i )+1 ] = black;
	  image[ 3*( y*width+x+i )+2 ] = black;
	}

    }

}
	  



