
/*
 * Use this function to initially get the most prominant feature
 * in each of the windows. If there are two points of equal
 * interest, there is no preference to which is chosen.
 *
 * The 'f' is filled in 
 */

void Detect_Prominent_Features(BYTE* col_im, BYTE *image, active *grid)
{

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

  IPLPoint point[XX*YY][MAX_POINTS];
  int      no_points[XX*YY];

  double *var = (double*) calloc( sizeof(double) * width * height, 1);

  motion = FALSE;

  memset(no_points, 0, XX*YY*sizeof(int));
  
  /* find interest in window but not on the edges! */
  interest(image, grid->main.x1, grid->main.y1, 
	   grid->main.x2, grid->main.y2, var);
      
  /* cycle through windows */
  for (k=0; k<XX*YY; k++)
      {

	f[k].moving = FALSE;
	
	x1 = grid->windows[k].x1; 
	y1 = grid->windows[k].y1;
	x2 = grid->windows[k].x2;
	y2 = grid->windows[k].y2;

	non_max_suppress(var, x1+SW, y1+SW, x2-SW, y2-SW,
			 point[k], &no_points[k]);

	if (no_points[k] == 0)
	  {
	    f[k].active = FALSE;
	    f[k].x = x1 + (x2-x1)/2;
	    f[k].y = y1 + (y2-y1)/2;
	    fprintf(stderr, "\nNo Prominent features (%d, %d, %d)", 
		    k, f[k].x, f[k].y);
	  }

	else
	  {
	    
	    f[k].active = TRUE;

	    /* take the point with maximum variance */	  
	    j=0;
	    for (i=1; i<no_points[k]; i++)
	      if ( var[ point[k][i].y*width + point[k][i].x ]
		      > var[ point[k][j].y*width + point[k][j].x ] )
		j=i;
		  
	    f[k].x      = point[k][j].x;
	    f[k].y      = point[k][j].y;
	    f[k].grad_x = grad_x(image, f[k].x, f[k].y);
	    f[k].grad_y = grad_y(image, f[k].x, f[k].y);

#ifdef COLOUR

	    f[k].r = smooth_pixel_col(col_im, G, f[k].x, f[k].y, 0);
	    f[k].g = smooth_pixel_col(col_im, G, f[k].x, f[k].y, 1);
	    f[k].b = smooth_pixel_col(col_im, G, f[k].x, f[k].y, 2); 

#else

	    f[k].r      = image[ f[k].y*width + f[k].x ];

#endif
	
	    f[k].var    = var[ point[k][j].y*width + point[k][j].x ];
	 
	  }
	
      }

  free(var);

}


/* 
 * NOT USED 
 */

void initial_moravec( BYTE * image, int x1, int y1, 
		    int x2, int y2, feature *f)
{

  int r = 1;  /* image reduction factor */

  int a,b,px,py;
  short i,j;
  double min, temp;
  double max_var = 0;
  
  for (i=x1; i<=x2; i+=r)
    for (j=y1; j<=y2; j+=r)
      {
	min = 0;
	temp = 0;

	for (a=i-M; a<i+M; a+=r)
	  for (b=j-M; b<=j+M; b+=r)
	      min += pow((double)image[b*width+a] 
			 - (double)image[b*width+a+r], 2);

	for (a=i-M; a<=i+M; a+=r)
	  for (b=j-M; b<j+M; b+=r)
	      temp += pow((double)image[b*width+a] 
		       - (double)image[(b+r)*width+a], 2);

	if (temp < min)
	  min = temp;

	temp = 0;

	/* forward diagonal */
	for (a=i-M; a<=i+M; a+=r)
	  for (b=j-M; b<j+M; b+=r)
	      temp += pow( (double)image[b*width+a] 
			   - (double)image[(b+r)*width+(a+r)], 2);

	if (temp < min)
	  min = temp;

	temp = 0;
	
	/* backward diagonal */
	for (a=i-M; a<=i+M; a+=r)
	  for (b=j-M; b<j+M; b+=r)
	      temp += pow( (double)image[b*width+a]
			   - (double)image[(b+r)*width+(a-r)], 2);

	if (temp < min)
	  min = temp;

	
	if (max_var <= min)
	  {
	    max_var = min;
	    px = i;
	    py = j;
	  }


      }

  /*********
	    GET REST OF ATTRIBUTES
  *******/
  
  f->x = px;
  f->y = py;
  f->grad_x = grad_x(image, px, py);
  f->grad_y = grad_y(image, px, py);
  f->var    = max_var;

}



/*
 * Caculates the interest of each pixel in the region
 * (x1, y1) -> (x2,y2). Returns a variance map 
 */

void  interest(BYTE * imageIn, int x1, int y1, 
	       int x2, int y2, double *var)
{

  int a,b,i,j;
  double temp, min;
  
#ifdef DEBUG

  if ( (x1-M < 0) || (y1-M < 0) || (x2+M >= width) || (y2+M >= height) )
    fprintf(stderr, "Problem: calculating interest out of bounds");
  
#endif

  /* clamp */
  if (x1-M < 0)
    x1 = M;
  
  if (y1-M < 0)
    y1 = M;

  if (x2+M >= width)
    x2 = width-M-1;
  
  if (y2+M >= height)
    y2 = height-M-1;

  for (i=x1; i<=x2; i+=R)
    for (j=y1; j<=y2; j+=R)
      {

	if (var[j*width+i] == 0)
	  {
	    
	    min = 0;
	    temp = 0;
	    
	    for (a=i-M; a<i+M; a+=R)
	      for (b=j-M; b<=j+M; b+=R)
		min += pow((double)imageIn[b*width+a] 
			   - (double)imageIn[b*width+a+R], 2);

	    for (a=i-M; a<=i+M; a+=R)
	      for (b=j-M; b<j+M; b+=R)
		temp += pow((double)imageIn[b*width+a] 
			    - (double)imageIn[(b+R)*width+a], 2);
	    
	    if (temp < min)
	      min = temp;
	    
	    temp = 0;
	    
	    /* forward diagonal */
	    for (a=i-M; a<=i+M; a+=R)
	      for (b=j-M; b<j+M; b+=R)
		temp += pow( (double)imageIn[b*width+a] 
			     - (double)imageIn[(b+R)*width+(a+R)], 2);
	    
	    if (temp < min)
	      min = temp;
	    
	    temp = 0;
	    
	    /* backward diagonal */
	    for (a=i-M; a<=i+M; a+=R)
	      for (b=j-M; b<j+M; b+=R)
		temp += pow( (double)imageIn[b*width+a]
			     - (double)imageIn[(b+R)*width+(a-R)], 2);
	    
	    if (temp < min)
	      min = temp;
	    
	    var[j*width+i] = min;
	  }
      }

}



/* Performs non-maximal suppression on the variance map
 * and returns the points in 'point' and the number of 
 * points in 'no_points'
 */

void non_max_suppress(double* var, int x1, int y1, int x2, int y2,
		      IPLPoint *point, int *no_points)
{

  int i,j,a,b,t;
    
#ifdef DEBUG

  if ( (x1 < SW) || (y1 < SW) || 
       (x2 > width-SW-1) || (y2 > height-SW-1) )
       fprintf(stderr, "Problem: going out of bounds here\n");

#endif

  for (i=x1; i<=x2; i++)
    for (j=y1; j<=y2; j++)
      {
	t=0;
	
	if ( var[j*width+i] > var_thres )
	  {
	    for (a=i-SW; a<=i+SW; a++)
	      for (b=j-SW; b<=j+SW; b++)
		if ( var[j*width+i] > var[b*width+a] )
		  t++;
	    
	    if ( t == supp_thres )
	      {
		point[*no_points].x = i;
		point[*no_points].y = j;
		
		(*no_points)++;
		
		if (*no_points >= MAX_POINTS)
		  fprintf(stderr, "\nPROBLEM: 
			  Maximum number of points exceeded\n");
	      }
	  }
      }
  
}


/*
 * This function finds the matching features in each image
 */

int find_features(BYTE* col_im, BYTE *image)
{

  int i,k;

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

  IPLPoint** point;
  int      no_points[XX*YY];

  double *var = (double*) calloc( sizeof(double) * width * height, 1);

 
  memset(no_points, 0, XX*YY*sizeof(int));
  
  point = (IPLPoint**) malloc( sizeof(IPLPoint*)*XX*YY );
  for (i=0; i<XX*YY; i++)
    point[i] = (IPLPoint*) malloc( sizeof(IPLPoint)*MAX_POINTS );

  for (k=0; k<XX*YY; k++)
    {

#ifdef MOTION
 	
      if ( (motion && f[k].moving) || !motion )
	{
	  
#endif
	  
	  x1 = f[k].x - s;
	  y1 = f[k].y - s;  
	  x2 = f[k].x + s;
	  y2 = f[k].y + s;
	  
	  /* clamp 
	  if (x1-M < 0)
	    x1 = M;
	  
	  if (y1-M < 0)
	    y1 = M;
	  
	  if (x2+M >= width)
	    x2 = width-M-1;
	  
	  if (y2+M >= height)
	  y2 = height-M-1; */

	  interest(image, x1, y1, x2, y2, var);
	
	  /* non-maximal suppression */
	  if (x1 < SW)
	    x1 = SW;
	
	  if (y1 < SW)
	    y1 = SW;
	  
	  if (x2 > width-SW-1)
	    x2 = width-SW-1;
	  
	  if (y2 > height-SW-1)
	    y2 = height-SW-1;
	  
	  non_max_suppress(var, x1+SW, y1+SW, x2-SW, y2-SW,
			   point[k], &no_points[k]);
	  
	  if (no_points[k] == 0)
	    f[k].active = FALSE;
	 
#ifdef MOTION

	  /* lost a moving feature - start again */
	  if ( (no_points[k] == 0) && (motion == TRUE) )  
	    {
	      first = TRUE;
	      motion = FALSE;
	      free(var);
	      for (i=0; i<XX*YY; i++)
		free(point[i]);
	      free(point);
	      return(FALSE);
	    }
	}

#endif
 
    }

  
#ifdef RELAX

  correspond2(col_im, image, point, no_points);

#else

  for (k=0; k<XX*YY; k++)
    if (f[k].active == TRUE)
      correspond(col_im, image, &f[k], point[k], no_points[k]);

#endif
  
  free(var);
  for (i=0; i<XX*YY; i++)
    free(point[i]);
  free(point);

  return(TRUE);

}


/*
 * NOT USED and NOT COMPLETE
 * If a feature is not being tracked and it has been lost
 * for MAX_LOST frames, then give up on it and find the 
 * most prominate feature in that area
 */
/*
update_features(IPLPoint** point, int* no_points, double *var)
{

  int k,i,j;
  

  for (k=0; k<XX*YY; k++)
   
    if ( (f[k].active == FALSE) && (f[k].lost >= MAX_LOST) 
	 && (no_points[k] != 0) )
      {
	
	j=0;
	for (i=1; i<no_points[k]; i++)
	  if ( var[ point[k][i].y*width + point[k][i].x ] >
	       var[ point[k][j].y*width + point[k][j].x ] )
	    j = i;
	
      }

}
*/
