
/*
 * Calculates the top left and bottom right 
 * coordinates of each of the windows and
 * the active area 
 */

void generate_windows(active* grid)
{

  int i,j;

  IPLPoint centre;

  int halfX;
  int halfY;

  centre.x = floor(width/2);
  centre.y = floor(height/2);
  halfX    = floor((XX*xx)/2);
  halfY    = floor((YY*yy)/2);
  grid->main.x1 = centre.x - halfX;
  grid->main.y1 = centre.y - halfY;
  grid->main.x2 = centre.x + halfX;
  grid->main.y2 = centre.y + halfY;

  for (i=0; i<XX; i++)
    for (j=0; j<YY; j++)
      {
	grid->windows[j*XX+i].x1 = grid->main.x1 + i*xx;
	grid->windows[j*XX+i].y1 = grid->main.y1 + j*yy;
	grid->windows[j*XX+i].x2 = grid->main.x1 + (i+1)*xx-1;
	grid->windows[j*XX+i].y2 = grid->main.y1 + (j+1)*yy-1;
      }
	  
}



/* 
 * Returns the x gradiant of the pixel (x,y)
 */

double   grad_x(BYTE * In, int x, int y)
{

  double result;

#ifdef DEBUG
  
  if ( (x < 2) || (x > width-2) || (y < 2) || (y > height-2) )
    fprintf(stderr, "grad_x: too close to edge\n");
  
#endif
  
  result = 
    -2*( (double)In[(y-2)*width+x-2] + (double)In[(y-1)*width+x-2] +
	 (double)In[y*width+x-2] + (double)In[(y+1)*width+x-2] +
	 (double)In[(y+2)*width+x-2] )
    -1*( (double)In[(y-2)*width+x-1] + (double)In[(y-1)*width+x-1] +
	 (double)In[y*width+x-1] + (double)In[(y+1)*width+x-1] +
	 (double)In[(y+2)*width+x-1] )
    +1*( (double)In[(y-2)*width+x+1] + (double)In[(y-1)*width+x+1] +
	 (double)In[y*width+x+1] + (double)In[(y+1)*width+x+1] +
	 (double)In[(y+2)*width+x+1] )
    +2*( (double)In[(y-2)*width+x+2] + (double)In[(y-1)*width+x+2] +
	 (double)In[y*width+x+2] + (double)In[(y+1)*width+x+2] +
	 (double)In[(y+2)*width+x+2] );

  return(result);

}



/*
 * Returns the y gradiant of the pixel (x,y)
 */

double   grad_y(BYTE * In, int x, int y)
{
 
  double result;
 
#ifdef DEBUG
  
  if ( (x < 2) || (x > width-2) || (y < 2) || (y > height-2) )
       fprintf(stderr, "grad_y: too close to edge\n");
  
#endif

  result = 
    -2*( (double)In[(y-2)*width+x-2] + (double)In[(y-2)*width+x-1] +
	 (double)In[(y-2)*width+x]   + (double)In[(y-2)*width+x+1] +
	 (double)In[(y-2)*width+x+2] )
    -1*( (double)In[(y-1)*width+x-2] + (double)In[(y-1)*width+x-1] +
	 (double)In[(y-1)*width+x]   + (double)In[(y-1)*width+x+1] +
	 (double)In[(y-1)*width+x+2] )
    +1*( (double)In[(y+1)*width+x-2] + (double)In[(y+1)*width+x-1] +
	 (double)In[(y+1)*width+x]   + (double)In[(y+1)*width+x+1] +
	 (double)In[(y+1)*width+x+2] )
    +2*( (double)In[(y+2)*width+x-2] + (double)In[(y+2)*width+x-1] +
	 (double)In[(y+2)*width+x]   + (double)In[(y+2)*width+x+1] +
	 (double)In[(y+2)*width+x+2] );
  
  return(result);
  
}


/*
 * Given a set of features, this function finds the active
 * area that encloses all feature points
 */

void find_active_area(active *grid)
{

  int i;

  int s = floor(SEARCH/2);

  grid->main.x1 = width;
  grid->main.y1 = width;
  grid->main.x2 = 0;
  grid->main.y2 = 0;

  for (i=0; i<XX*YY; i++)
    
    if (f[i].active == TRUE)
      {
	
	if (f[i].x < grid->main.x1)
	  grid->main.x1 = f[i].x;
	
	else if (f[i].x > grid->main.x2)
	  grid->main.x2 = f[i].x;
	
	if (f[i].y < grid->main.y1)
	  grid->main.y1 = f[i].y;
	
	else if (f[i].y > grid->main.y2)
	  grid->main.y2 = f[i].y;
	
      }
  
  grid->main.x1 -= s;
  grid->main.y1 -= s;
  grid->main.x2 += s;
  grid->main.y2 += s;
  
  if (grid->main.x1 < 0)
    grid->main.x1 = 0;

  if (grid->main.y1 < 0)
    grid->main.y1 = 0;
  
  if (grid->main.x2 >= width)
    grid->main.x2 = width-1;
  
  if (grid->main.y2 >= height)
    grid->main.y2 = height-1;
  
}
	  
      
/*
 * Each feature in the previous image is matched to a feature
 * found in the current image
 *
 * This is the corresondance method implemented by Gu
 */

void correspond(BYTE* col_im, BYTE* image, feature* s,
		IPLPoint *point, int no_points)
{

  int i,j;

  double *mismatch;
      
  double weights[] = { XNY,   /* x,y */ 
		       GRADS, /* grad x,y */
		       INTS   /* intensity or rgb when using colour */
  };
  

  double nd, n1, n2;

  feature* t;

  if (no_points != 0)
    {
      
      mismatch = (double*)  malloc (sizeof(double)*no_points);
      t        = (feature*) malloc (sizeof(feature)*no_points);

      for (i=0; i<no_points; i++)
	{
	  
	  t[i].x = point[i].x;
	  t[i].y = point[i].y;

#ifdef COLOUR

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

#else 

	  t[i].r = image[ t[i].y*width+t[i].x];

#endif

	  t[i].grad_x = grad_x(image, t[i].x, t[i].y);
	  t[i].grad_y = grad_y(image, t[i].x, t[i].y);
	  
	  n1 = norm_feature(&t[i], weights);
	  n2 = norm_feature(s,  weights);
	  nd = norm_diff_feature(s, &t[i], weights);
	  
	  mismatch[i] = nd / sqrt( n1 * n2 );
	  	  
	}
      /*
      fprintf(stderr, "\n----");
      
      for (i=0; i<no_points; i++)
	fprintf(stderr, "\n(%d,%d) (%d, %d), (%d, %d), (%d, %d), (%d, %d), (%1.3g, %1.3g), (%1.3g, %1.3g)",
		s->x, t[i].x,
		s->y, t[i].y,
		s->r, t[i].r,
		s->g, t[i].g,
		s->b, t[i].b,
		s->grad_x, t[i].grad_x,
		s->grad_y, t[i].grad_y);

      fprintf(stderr, "\n*****");
      */
      /* find the minimum */
      j=0;
      for (i=1; i<no_points; i++)
	if (mismatch[i] < mismatch[j])
	  j = i;

      //fprintf(stderr, "(j:%d)", j);

      /* compute error - only for a single image*/
      //if ( (s->x - point[j].x != 0) || (s->y - point[j].y) != 0)
      //fprintf(stderr, "(ERROR)");
      
      s->x   = t[j].x;
      s->y   = t[j].y;
      s->r   = t[j].r;

#ifdef COLOUR
      s->g   = t[j].g;
      s->b   = t[j].b;
#endif

      s->grad_x = t[j].grad_x;
      s->grad_y = t[j].grad_y;
      
      free(t);
      free(mismatch);

    }
  

 
}      


/*
 * computes the norm of a feature vector using the 
 * weights 'w'
 */

double norm_feature(feature *a, double *w)
{

#ifdef COLOUR
  
  return ( sqrt( (double)(w[0]*a->x*w[0]*a->x) +
		 (double)(w[0]*a->y*w[0]*a->y) +
		 (w[1]*a->grad_x*w[1]*a->grad_x) +
		 (w[1]*a->grad_y*w[1]*a->grad_y) +
		 (double)(w[2]*a->r*w[2]*a->r) +
		 (double)(w[2]*a->g*w[2]*a->g) +
		 (double)(w[2]*a->b*w[2]*a->b) ) );

#else /* using grey levels */ 
  
   return ( sqrt( (double)(w[0]*a->x*w[0]*a->x) +
		  (double)(w[0]*a->y*w[0]*a->y) +
		  (w[1]*a->grad_x*w[1]*a->grad_x) +
		  (w[1]*a->grad_y*w[1]*a->grad_y) + 
		  (double)(w[2]*a->r*w[2]*a->r) ) );
  
#endif 

}



/*
 * computes the difference between two feature vectors and
 * then computes the norm given weights 'w'.
 */

double norm_diff_feature(feature *a, feature *b, double *w)
{
  
#ifdef COLOUR
  
  return( sqrt( (double)( w[0]*(a->x - b->x)*w[0]*(a->x - b->x) ) + 
		(double)( w[0]*(a->y - b->y)*w[0]*(a->y - b->y) ) +
		(w[1]*(a->grad_x - b->grad_x)*w[1]*(a->grad_x - b->grad_x)) +
		(w[1]*(a->grad_y - b->grad_y)*w[1]*(a->grad_y - b->grad_y)) +
		(double)(w[2]*(a->r - b->r)*w[2]*(a->r - b->r))  +
		(double)(w[2]*(a->g - b->g)*w[2]*(a->g - b->g))  +
		(double)(w[2]*(a->b - b->b)*w[2]*(a->b - b->b)) ) );

#else
  
  return( sqrt( (double)( w[0]*(a->x - b->x)*w[0]*(a->x - b->x) ) + 
		(double)( w[0]*(a->y - b->y)*w[0]*(a->y - b->y) ) +
		(w[1]*(a->grad_x - b->grad_x)*w[1]*(a->grad_x - b->grad_x)) +
		(w[1]*(a->grad_y - b->grad_y)*w[1]*(a->grad_y - b->grad_y)) +
		(double)(w[2]*(a->r - b->r)*w[2]*(a->r - b->r)) ) );

#endif


}


/*
 * This is a more complicated version of correspondance as
 * discussed by Jain. It uses an interative relaxation
 * scheme
 */
  
void correspond2(BYTE* col_im, BYTE* image, IPLPoint** point, int* no_points)
{
  /* calculate mismatches and similarity */
  similarity(col_im, image, point, no_points);

  calc_probs(no_points);
  
  /* iterative relaxation based on consistency */
  relax(point, no_points);
  
  /* choose highest probability and correspond*/
  choose(point, no_points);

  if (motion == FALSE)
    if ( still_tracking() == TRUE )
      activate_lost(point, no_points);
  
}


/*
 * Adjusts the probabilities based on consistency
 */

void relax(IPLPoint **point, int* no_points)
{

  int i,k,dx,dy,j;

  double sum;
  double q[XX*YY][MAX_POINTS];

  IPLPoint disparity;

  for (j=0; j<ITERATIONS; j++)

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

#ifdef MOTION
 	
      if ( (motion && f[k].moving) || !motion )
	  
#endif
	 	
	if (f[k].active == TRUE)

	  if (no_points[k] != 0)              /* and there are candidates */
	    {
	      
	      for (i=0; i<no_points[k]; i++)  /* for those candidates */
		{
		  
		  q[k][i] = 0;
		  
		  disparity.x = f[k].x - t[k][i].x;
		  disparity.y = f[k].y - t[k][i].y;
		  
		  for (dx=-1; dx<=1; dx++)     /* consider 8 adjacent nodes */
		    for (dy=-1; dy<=1; dy++)
		      q[k][i] += sim_disparity(k,dx,dy, &disparity,
					       point, no_points);
		}
	      
	      /* update probabilities */
	    
	      for (i=0; i<no_points[k]; i++)
		p[k][i] = p[k][i]*(AA + BB*q[k][i]);
	      
	      /* normalize */
	      sum = 0;
	      for (i=0; i<no_points[k]; i++)
		sum += p[k][i];
	      
	      for (i=0; i<no_points[k]; i++)
		p[k][i] = p[k][i]/sum;
	      
	    }
	
      }
  
}


/*
 * returns, if there is one, the probability of a similar
 * disparity
 */
 
double sim_disparity(int k, int dx, int dy, IPLPoint* disparity,
		    IPLPoint** point, int* no_points)
{

  int i,j,x,y,kk;

  double mismatch[MAX_POINTS];

  IPLPoint temp;

  x = k%XX;
  y = (k-x)/XX;
  
  /* check to see if it is a neighbour */
  kk = (y+dy)*XX+(x+dx);
  if ( (kk == k) || (x+dx < 0) 
       || (y+dy < 0) || (x+dx >= XX) || (y+dy >= YY) 
       || f[kk].active == FALSE)
    return(0);

  /* find the label with the nearest dispartity 
     i.e. lowest mismatch */
  if ( f[kk].active == TRUE )
    {
      for (i=0; i<no_points[kk]; i++)
	{
	  temp.x = f[kk].x - point[kk][i].x;
	  temp.y = f[kk].y - point[kk][i].y;
	  
	  mismatch[i] = disparity_mismatch(disparity, &temp);
	  
	}
      
      j=0;
      for (i=1; i<no_points[kk]; i++)
	if (mismatch[i] < mismatch[j])
	  j=i;

      /* is it about the threshold */
      if (mismatch[j] <= MISMATCH_THRES)
	return( p[kk][j] );
      else
	return(0);
    }
  
  return(0);

}



/* 
 * calculates the mismatch of two disparities
 */

double disparity_mismatch(IPLPoint* a, IPLPoint* b)
{
  
  return( sqrt( (a->x - b->x)*(a->x - b->x) + 
	    (a->y - b->y)*(a->y - b->y) ) );

}



/*
 * Matches each feature with the candidate of highest probability
 * of being a match 
 */

void choose(IPLPoint ** point, int *no_points)
{
  
  int i,k,j;
  
  double prob_thres;

  for (k=0; k<XX*YY; k++)
    {
      
      if (f[k].active == TRUE)
	{
	  j=0;
	  for (i=1; i<no_points[k]; i++)
	    if (p[k][i] > p[k][j])
		j = i;
	
	  //prob_thres = (1/(double)no_points[k])*PROB_PROP; 
	  prob_thres = pow( (1/(double)no_points[k]), PROB_PROP);
	  
	  /* is it above the threshold */
	  if (p[k][j] < prob_thres)
	    {
	      fprintf(stderr, "\nBelow thres (%d)", k);
	      f[k].active = FALSE;

#ifdef MOTION
	
	      if (motion == TRUE) /* start again */
		first = TRUE;

#endif

	    }
	  else 
	    {
	      
	      /* begin REMOVE */
	      //if ( f[k].active == TRUE )
	      //if ( (f[k].x - point[k][j].x != 0) || 
	      //     (f[k].y - point[k][j].y) != 0)
	      //  fprintf(stderr, "ERROR %d", k);
	      /* end REMOVE*/

	      f[k].x   = t[k][j].x;
	      f[k].y   = t[k][j].y;
	      f[k].r   = t[k][j].r;
	      
#ifdef COLOUR
	      f[k].g   = t[k][j].g;
	      f[k].b   = t[k][j].b;
#endif
	      
	      f[k].grad_x = t[k][j].grad_x;
	      f[k].grad_y = t[k][j].grad_y;
	      
	    }
	}

    }
}
 

/*
 * This function calculates the similarity between each feature and all
 * it possible canditates
 */

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

  int i,k;
    
  double nd, n1, n2;
  
  double weights[] = { XNY,   /* x,y */ 
		       GRADS, /* grad x,y */
		       INTS   /* intensity or rgb when using colour */
  };
  


  for (k=0; k<XX*YY; k++)
    {
      
#ifdef MOTION
      if ( (motion && f[k].moving) || !motion )
#endif
	  
	if (no_points[k] != 0)
	  {
	    
	    for (i=0; i<no_points[k]; i++)
	      {
		
		t[k][i].x = point[k][i].x;
		t[k][i].y = point[k][i].y;

#ifdef COLOUR
	      
		t[k][i].r = smooth_pixel_col(col_im, G, 
					     t[k][i].x, t[k][i].y, 0);
		t[k][i].g = smooth_pixel_col(col_im, G, 
					     t[k][i].x, t[k][i].y, 1);
		t[k][i].b = smooth_pixel_col(col_im, G, 
					     t[k][i].x, t[k][i].y, 2); 
		
#else 
		
		t[k][i].r = image[ t[k][i].y*width+t[k][i].x];
		
#endif
		
		t[k][i].grad_x = grad_x(image, t[k][i].x, t[k][i].y);
		t[k][i].grad_y = grad_y(image, t[k][i].x, t[k][i].y);
		
		n1 = norm_feature(&t[k][i], weights);
		n2 = norm_feature(&f[k],  weights);
		nd = norm_diff_feature(&f[k], &t[k][i], weights);
		
		similar[k][i] = 1 / ( 1 + CC * (nd / sqrt( n1 * n2 )) );
		
	      }
	    
	    
	  }
    }
  
}



/*
 * This function simply forms probability of each canditate being a 
 * match to the feature based solely on the similarity
 */

void calc_probs(int* no_points)
{

  int k, i;

  double sum;

  for (k=0; k<XX*YY; k++)
    {
      
#ifdef MOTION
      if ( (motion && f[k].moving) || !motion )
#endif
	
      if (no_points[k] != 0)
	{
	  
	  sum = 0;
	  
	  for (i=0; i<no_points[k]; i++)
	    sum += similar[k][i];

	  for (i=0; i<no_points[k]; i++)
	    p[k][i] = similar[k][i]/sum;
	  
	}

    }

}
    


/*
 * Determines if tracking is still possible 
 * Checks to see if there is more than CRITICAL no
 * of features being tracked. If greater, then
 * first becomes true and tracking starts over
 */

int still_tracking()
{

  int k,sum=0;

  for ( k=0; k<XX*YY; k++ )
    if (f[k].active == FALSE)
      sum++;

  if (sum > CRITICAL)
    {
      fprintf(stderr, "\nTracking Lost . . . re-acquiring (%d)", sum);
      first = TRUE;
      return(FALSE);
    }

  return(TRUE);
  
}

/*
 * If a feature is lost, then look for it (or another feature)
 * in the same spot 
 */

void activate_lost(IPLPoint **point, int* no_points)
{

  int k,i,j;
  
  double prob_thres;

  /* update feature attributes */
  for ( k=0; k<XX*YY; k++ )
    if ( f[k].active == FALSE )
      {
  	
	/* find the most probable */
  	if (no_points[k] != 0)
	  {

	    j = 0;
	    for (i=1; i<no_points[k]; i++)
	      if (p[k][i] > p[k][j])
		j = i;

	    //prob_thres = (1/(double)no_points[k])*PROB_PROP;
	    prob_thres = pow( (1/(double)no_points[k]), PROB_PROP);

	    if (p[k][j] >= prob_thres)  /* put in new features */
	      {
		
		fprintf(stderr, "\nadded %d", k);

		f[k].x = t[k][j].x;
		f[k].y = t[k][j].y;
		f[k].grad_x = t[k][j].grad_x;
		f[k].grad_y = t[k][j].grad_y;
		f[k].r = t[k][j].r;
#ifdef COLOUR
		f[k].g = t[k][j].g;
		f[k].b = t[k][j].b;
#endif
		f[k].active = TRUE;
	      }
	    
	  }
	
      }
}



/* 
 * determines if there is moving target and if so
 * will track only those features that are on the 
 * moving target
 */

void detect_motion()
{


  int k,n=0;
  double d;

  /* only detect if all points avaliable */
  for (k=0; k<XX*YY; k++)
    if ( f[k].active == FALSE )
      return;
      
  for (k=0; k<XX*YY; k++)
    {

      if ( f[k].active == TRUE)
	{
	  d = sqrt(  (double)(f[k].x - g[k].x)*(double)(f[k].x - g[k].x) +
		     (double)(f[k].y - g[k].y)*(double)(f[k].y - g[k].y) );
	  
	  if (d > MOTION_DTHRES)
	    {
	      f[k].moving = TRUE;
	      n++;
	    }
	  else 
	    f[k].moving = FALSE;

	}
    }
  
  if (n >= MOTION_NO_FEATS)
    {
      fprintf(stderr, "MOTION");
      for (k=0; k<XX*YY; k++)
	if ( f[k].moving == FALSE )
	  f[k].active = FALSE;
	
      motion = TRUE;
    }

  
}



/*
 * Start tracking once there are a sufficent
 * number of features to track. i.e number
 * of inactive features is less than CRITICAL
 * RETURNS false when tracking can start
 */

int can_track()
{

  int i,n=0;

  
  for (i=0; i<XX*YY; i++)
  
    {
      if ( f[i].active == FALSE )
	n++;

      if ( n == CRITICAL )
	return(TRUE);
       
    }

  return(FALSE);
  
}




