

#include <math.h>
#include "canny.h"
#include "vector.h"
#include "gradient.h"
#include "smooth.h"

extern double sqrt(double);
extern double exp(double);


static int canny_edge(register float *dx, register float *dy, register float *magn,
		      const long width, const long height, const float thresh,
		      register uchar *output);
static int canny_edge2(register float *dx, register float *dy, register float *magn,
		       const long width, const long height, const float thresh,
		       const float thresh2, uchar *output);


/* Constants for the noise estimate routine */

#define NOISE_BUCKETS    1000		     /* number of histogram buckets */
#define NOISE_RANGE      0.15		     /* relative range of data to hist. */
					     /* (eg. values <= 15% of max val) */
#define NOISE_MAX_SIGMA 15.00		     /* absolute */
#define NOISE_MIN_SIGMA  5.00		     /* absolute */
#define NOISE_SIGMA      0.10		     /* 1/10th of initial estimate */


/* Global parameters and variables */

float       CannyNoiseEstimate = 0.0;
float       CannyThreshold1 = 0.0, CannyThreshold2 = 0.0;



/* Functions */


        
BinaryImage cannyEdges_(FloatImage img, float thresh1, float thresh2,
                        kwArgDecl(CannyKW))
{
  kwDefault(mutate,    FALSE);
  kwDefault(relative,  FALSE);
  kwDefault(recompute, FALSE);
  kwDefaultN(dxp,   dx,   ((FloatImage *) NULL));
  kwDefaultN(dyp,   dy,   ((FloatImage *) NULL));
  kwDefaultN(magp,  mag,  ((FloatImage *) NULL));
  kwDefaultN(edges, into, ((BinaryImage)  NULL));

  FloatImage dx, dy, mag;
  boolean made_edges = FALSE;
  

  if (img == (FloatImage) NULL)
    return (BinaryImage) NULL;

  dx  = (dxp  == (FloatImage *) NULL) ? NULL : *dxp;
  dy  = (dyp  == (FloatImage *) NULL) ? NULL : *dyp;
  mag = (magp == (FloatImage *) NULL) ? NULL : *magp;

  if (edges == (BinaryImage) NULL) {
    edges = (BinaryImage) imNew(IMAGE_BINARY, imGetWidth(img), imGetHeight(img));

    if (edges == (BinaryImage) NULL)
      return (BinaryImage) NULL;

    made_edges = TRUE;
  }
  
  if (recompute   ||
      dx == (FloatImage) NULL || dy == (FloatImage) NULL || mag == (FloatImage) NULL) {
    if (gradientFull(img, &dx, &dy, &mag)) {
      if (made_edges)
	imFree(edges);
      
      return (BinaryImage) NULL;
    }
  }

  if (relative) {                            /* thresholds are relative => get noise */
    float noise;			     /* and scale them by it. */
    
    noise = cannyNoiseEstimate(mag);
    
    if (noise < 0.0) {			     /* error getting noise estimates */
      if (dxp  == (FloatImage *) NULL  ||  *dxp  == (FloatImage) NULL)   imFree(dx);
      if (dyp  == (FloatImage *) NULL  ||  *dyp  == (FloatImage) NULL)   imFree(dy);
      if (magp == (FloatImage *) NULL  ||  *magp == (FloatImage) NULL)   imFree(mag);
      if (made_edges) imFree(edges);
      return (BinaryImage) NULL;
    }

    thresh1 *= noise;
    thresh2 *= noise;
  }

  CannyThreshold1 = thresh1;		     /* save values in global vars */
  CannyThreshold2 = thresh2;
  
  /* note: thresholds need to be squared since they should be thresholds for
   * the gradient magnitude squared * 4.0.  Be sure to preserve the signs of
   * the thresholds, since thresh1 <= 0 means no thresholding, and thresh2 <
   * 0.0 means only one threshold.
   */
  
  thresh1 = thresh1 * thresh1 * 4.0 * ((thresh1 < 0.0) ? -1.0 : 1.0);
  thresh2 = thresh2 * thresh2 * 4.0 * ((thresh2 < 0.0) ? -1.0 : 1.0);

  if (thresh1 <= 10.E-5)  thresh1 = 1.0E-5;  /* ensure thresholds > 0.0 */

  if (thresh2 < 0.0) {			     /* only one threshold specified */
    canny_edge(imGetStore(dx), imGetStore(dy), imGetStore(mag), imGetWidth(img),
	       imGetHeight(img), thresh1, imGetStore(edges));
  }
  else {				     /* two thresholds were specified */
    if (thresh2 <= 1.0E-5)  thresh2 = 1.0E-5;
    if (thresh1 > thresh2)  SWAP(thresh1, thresh2); /* swap 'em if necessary */
    
    canny_edge2(imGetStore(dx), imGetStore(dy), imGetStore(mag), imGetWidth(img),
		imGetHeight(img), thresh1, thresh2, imGetStore(edges));
  }

  if (mutate) {
    char *edge_ptr, *edge_end;
    float *mag_ptr;

    edge_ptr = imGetStore(edges);
    edge_end = edge_ptr + imGetStoreLen(edges);
    mag_ptr  = imGetStore(mag);

    while (edge_ptr < edge_end) {
      *mag_ptr = (*edge_ptr) ? sqrt(*mag_ptr) / 2.0 : 0.0;

      mag_ptr++;
      edge_ptr++;
    }
  }

  if (dxp == (FloatImage *) NULL)       imFree(dx);
  else if (*dxp == (FloatImage) NULL)   *dxp = dx;

  if (dyp == (FloatImage *) NULL)       imFree(dy);
  else if (*dyp == (FloatImage) NULL)   *dyp = dy;

  if (magp == (FloatImage *) NULL)      imFree(mag);
  else if (*magp == (FloatImage) NULL)  *magp = mag;

  return edges;  
}


/* This is based on Chapter 2 of Harry Voorhees' SM Thesis
 * "Finding Testure Boundaries In Images," AI Tech Rpt 968/
 *
 * The histogram of the gradient magnitude should be a Rayleigh
 * distribution:  f(x) = (x/a)*(e^(-x^2/2a^2)) and F(x) = 1 - e^(-x^2/2a^2)
 * Our noise estimate is the peak of f(x), which occurs when x = a.
 */

#define DBG_NE 0

float cannyNoiseEstimate(FloatImage mag)     /* given gradient magnitude squared */
{					     /* (times 4) of the image, give an */
  static FloatVector hist  = (FloatVector) NULL;  /* estimate of the noise */
  static FloatVector hists = (FloatVector) NULL;
  static FloatVector histd = (FloatVector) NULL;
  register float *scan, *end, tmp, scale, mxval;
  register int index;
#if DBG_NE
  float max_bucket_value;
  int max_bucket_index;
#endif 
  int initial;
  float noise, sum, cdf;
  float sigma;
  FloatVector gaussian;
  int i, j, len;
  float minderiv, maxderiv, deriv;
  int estimate;

  if (hist == (FloatVector) NULL) {	     /* allocate histograms if needed */
    if ((hist  = (FloatVector) vecNew(VECTOR_FLOAT, NOISE_BUCKETS)) == NULL  ||
	(hists = (FloatVector) vecNew(VECTOR_FLOAT, NOISE_BUCKETS)) == NULL  ||
	(histd = (FloatVector) vecNew(VECTOR_FLOAT, NOISE_BUCKETS)) == NULL) {
      return -1.0;
    }
  }

  vecInit(hist, 0.0);			     /* clear histogram */
  
  mxval = -1.0;				     /* Get maximum gradient magnitude value */
  for (scan = imGetStore(mag), end = scan + imGetStoreLen(mag); scan < end; scan++) {
    if ((tmp = *scan) > mxval)
      mxval = tmp;
  }

  mxval *= NOISE_RANGE * NOISE_RANGE;	     /* max mag-sqrd value to histogram */
  scale  = NOISE_BUCKETS / sqrt(mxval);	     /* scale for histograming mag vals */
  
					     /* now histogram the data */
  for (scan = imGetStore(mag), end = scan + imGetStoreLen(mag); scan < end; scan++) {
    if ((tmp = *scan) <= mxval) {	     /* ok to histogram? */
      vecRef(hist, ((int) (sqrt(tmp)*scale))) += 1.0; /* yes .. scale & histogram */
      sum += 1.0;
    }
  }
  
  /* partly kill the peak there *//* maybe use init est to figure how many to kill */
  vecRef(hist, 0) = vecRef(hist, 1) = vecRef(hist, 2) = 0.0;

  for (sum = 0.0, initial = 0; initial < NOISE_BUCKETS; initial++) {
    sum += vecRef(hist, initial);
  }
  
#if DBG_NE
    fprintf(stderr, "sum = %f  sum * .40 = %f\n", sum, sum*0.40);
#endif

  sum *= 0.40;
  for (cdf = 0.0, initial = 0; initial < NOISE_BUCKETS && cdf < sum; initial++) {
    cdf += vecRef(hist, initial);
  }

#if DBG_NE
    fprintf(stderr, "cdf = %f  initial index est. = %d\n", cdf, initial);
#endif

  noise = (initial + 0.5)/scale/2.0;

#if DBG_NE
  fprintf(stderr, "Initial noise estimate is:  %f\n", noise);
#endif


#if DBG_NE				     /* output original histogram */
  { BinaryImage plot = (BinaryImage) imNewOffset(IMAGE_BINARY,
						 NOISE_BUCKETS+2, 402, -1, -1);
    
    max_bucket_value = -1.0;
    for (index = 0; index < NOISE_BUCKETS; index++) {
      tmp = vecRef(hist, index);
      if (tmp >= max_bucket_value) {
	max_bucket_value = tmp;
      }
    }
    fprintf(stderr, "y-scale for histogram: 380 = %f\n", max_bucket_value);
    for (index = 0; index < NOISE_BUCKETS; index++) {
      int y = vecRef(hist, index) * 380.0 / max_bucket_value;
      imRef(plot, index, y)   = 1;
      imRef(plot, index+1, y) = 1;      imRef(plot, index, y+1) = 1;
      imRef(plot, index-1, y) = 1;      imRef(plot, index, y-1) = 1;
    }

    fprintf(stderr, "scale = %f\n", scale);

    imSave(plot, "noise-plot.pbm");    imFree(plot);
  }
#endif

  /* smooth the histogram and re-compute the index of the maximal bucket */
  sigma = MIN(NOISE_MAX_SIGMA, MAX(NOISE_SIGMA*initial, NOISE_MIN_SIGMA));
  if ((gaussian = smoothMakeGaussianMask(sigma, 2.5)) == (FloatVector) NULL) {
    return -1.0;
  }
  
#if DBG_NE
    fprintf(stderr, "sigma = %f\n", sigma);
#endif

  vecRef(hist, 0) = 0.0;
  len = vecGetLength(gaussian);
  for (i = 0; i < NOISE_BUCKETS; i++) {
    sum = vecRef(gaussian, 0) * vecRef(hist, i);
    for (j = 1; j < len; j++) {
      sum += vecRef(gaussian, j)*((i-j >= 0 ? vecRef(hist, i-j) : -vecRef(hist, j-i)) +
				  (i+j < NOISE_BUCKETS ? vecRef(hist, i+j) : 0.0));
    }
    vecRef(hists, i) = sum;
  }
  vecFree(gaussian);

#if DBG_NE
  max_bucket_value = -1.0;		     /* find max bucket value & index */
  max_bucket_index = -1;
  for (index = 0; index < NOISE_BUCKETS; index++) {
    tmp = vecRef(hists, index);
    if (tmp >= max_bucket_value) {
      max_bucket_value = tmp;
      max_bucket_index = index;
    }
  }

  fprintf(stderr, "peak at %d\n", max_bucket_index);

  noise = (max_bucket_index + 0.5)/scale/2.0;
#endif

  /* derivative based approach */
  minderiv = maxderiv = 0.0;		     /* compute histogram derivative */
  vecRef(histd, 0) = 0.0;
  for (index = 1; index < NOISE_BUCKETS; index++) {
    deriv = vecRef(histd, index) = vecRef(hists, index) - vecRef(hists, index-1);
    
    if (deriv > maxderiv) maxderiv = deriv;
    if (deriv < minderiv) minderiv = deriv;
  }

  /* now use as estimate the beginning of the largest region of negative
     valued derivative. */
  
  { int nareas = 0, begin;
    typedef struct { int begin; float area; } AREA;
    AREA areas[2];
    float area;
    
    
    for (i = 0; i < NOISE_BUCKETS - 1; i++) {
      if (vecRef(histd, i) >= 0.0  && vecRef(histd, i+1) < 0.0) {
	begin = ++i;
	area  = vecRef(histd, i);
	for ( ; i < NOISE_BUCKETS - 1 && vecRef(histd, i) < 0.0; i++) {
	  area += vecRef(histd, i);
	}
	area = -area; i--;
	areas[nareas >= 2
	      ? (areas[0].area < areas[1].area
		 ? 0
		 : 1)
	      : nareas++] = ((AREA) {begin: begin, area: area});
      }
    }

    estimate = (nareas == 2
		? (areas[0].area > areas[1].area
		   ? 0
		   : 1)
		: 0);
    estimate = areas[estimate].begin;
    noise = (estimate + 0.5)/scale/2.0;

#if DBG_NE
    fprintf(stderr, "noise = %f   estimate = %d\n", noise, estimate);
#endif 
  }
      

#if DBG_NE				     /* output new histogram */
  { BinaryImage plot = (BinaryImage) imNewOffset(IMAGE_BINARY, NOISE_BUCKETS+2, 402,
						 -1, -1);
    int y0;
     
    fprintf(stderr, "Better noise estimate is:  %f\n", noise);

    for (index = 0; index < NOISE_BUCKETS; index++) {
      int y = vecRef(hists, index) * 380.0 / max_bucket_value;
      imRef(plot, index, y)   = 1;
      imRef(plot, index+1, y) = 1;      imRef(plot, index, y+1) = 1;
      imRef(plot, index-1, y) = 1;      imRef(plot, index, y-1) = 1;

      y = ((380.0*vecRef(hists, estimate)*estimate*1.6487212707/max_bucket_value) *
	   ((float) index / (estimate * estimate))  *
	   exp(-(index*index)/(2.0*estimate*estimate)));
      imRef(plot, index, y)   = 1;
      imRef(plot, index+1, y+1) = 1;      imRef(plot, index-1, y+1) = 1;
      imRef(plot, index-1, y-1) = 1;      imRef(plot, index+1, y-1) = 1;
    }

    y0 = (0.0 - minderiv) * 399.0 / (maxderiv - minderiv);
    for (index = 0; index < NOISE_BUCKETS; index++) {
      int y = (vecRef(histd, index) - minderiv) * 399.0 / (maxderiv - minderiv);

      imRef(plot, index, y)   = 1;
      imRef(plot, index+1, y) = 1;      imRef(plot, index, y+1) = 1;
      imRef(plot, index-1, y) = 1;      imRef(plot, index, y-1) = 1;
      imRef(plot, index, y0)  = 1;
    }

    for (j = 0; j < 400; j++) {
      if (j & 8) imRef(plot, estimate, j) = 1;
      if (j & 4) imRef(plot, max_bucket_index, j) = 1;
    }
    imSave(plot, "noise-plot2.pbm");    imFree(plot);
  }
#endif
  
  CannyNoiseEstimate = noise;		     /* save in global var */
  return noise;  
}



/******************************* Local Functions *******************************/

static long offset1[5];
static long offset2[5];

static inline int NMS(const float *magn, const float mag, const float dx, const float dy)
{
  float theta, m1, m2;
  register int n, o1, o2;
  float interp, interp2;

					     /* map [-pi/2 .. pi/2] to [0.0 .. 4.0] */
  if (dx == 0.0)			     /* Don't divide by 0.0 */
    theta = 0.0;			     /* 90 degrees */
  else
    theta = 4.0*(0.5 + atan(dy/dx)/M_PI);	     /* 4*(.5 + PI*theta) */

  n = (int) theta;			     /* region we're in */
  interp = theta - (float) n;		     /* interpolating factors */
  interp2 = 1.0 - interp;

  o1 = offset1[n];			     /* get offsets & interpolate */
  o2 = offset2[n];
  m1 = *(magn + o1)*interp2 + *(magn + o2)*interp;
  m2 = *(magn - o1)*interp2 + *(magn - o2)*interp;

  return ((mag>=m1 && mag>m2) || (mag>m1 && mag>=m2)); /* return 1 iff passes NMS */
}


static int canny_edge(register float *dx, register float *dy, register float *magn,
		      const long x_m, const long y_m, const float thresh,
		      register uchar *output)
{
  float *magn_max, mag;
  register float *magn_max_x;


  offset1[0] = x_m;   offset2[0] = x_m-1;    /* 225..270 and 45..90 */
  offset1[1] = x_m-1; offset2[1] = -1;	     /* 180..225 and 0..45 */
  offset1[2] = 1;     offset2[2] = x_m+1;    /* 315..360 and 135..180 */
  offset1[3] = x_m+1; offset2[3] = x_m;	     /* 270..315 and 90..135 */ 
  offset1[4] = x_m;   offset2[4] = x_m-1;    /* 225..270 and 45..90 */

  output += x_m + 1;			     /* skip first row */
  dx     += x_m + 1;			     /* leave zero=>no edge */
  dy     += x_m + 1;
  
  for (magn_max = magn + x_m*(y_m - 1) - 1, magn += x_m + 1; magn < magn_max; ) {
    for (magn_max_x = magn + x_m - 2; magn < magn_max_x;
	 output++, dx++, dy++, magn++)
    {
      if ((mag = *magn) < thresh)	     /* don't even need to do NMS */
	*output = 0;
      else				     /* interpolate gradient & do NMS */
	*output = NMS(magn, mag, *dx, *dy);
    }

    output += 2;			     /* skip over last pixel on this line */
    dx     += 2;			     /* and first pixel on next line */
    dy     += 2;
    magn   += 2;
  }

  return 0;
}


static int canny_edge2(register float *dx, register float *dy, register float *magn,
		       const long x_m, const long y_m, const float thresh,
		       const float thresh2, uchar *output)
{
  register uchar *out;
  float *magn_max, *magn_max_x;
  float mag;
  uchar **stack, **stktop;


  offset1[0] = x_m;   offset2[0] = x_m-1;    /* 225..270 and 45..90 */
  offset1[1] = x_m-1; offset2[1] = -1;	     /* 180..225 and 0..45 */
  offset1[2] = 1;     offset2[2] = x_m+1;    /* 315..360 and 135..180 */
  offset1[3] = x_m+1; offset2[3] = x_m;	     /* 270..315 and 90..135 */ 
  offset1[4] = x_m;   offset2[4] = x_m-1;    /* 225..270 and 45..90 */

  stktop = stack = (uchar **) malloc(x_m * y_m * sizeof(uchar *)); /* get a stack */

  if (stack == (uchar **) NULL)
    return -1;
  
  out  = output;
  out += x_m + 1;			     /* skip over first row */
  dx  += x_m + 1;
  dy  += x_m + 1;

  for (magn_max = magn + x_m*(y_m - 1) - 1, magn += x_m + 1; magn < magn_max; ) {
    for (magn_max_x = magn + x_m - 2; magn < magn_max_x;
	 out++, dx++, dy++, magn++)
    {
      if ((mag = *magn) < thresh)	     /* no possible edge */
	*out = 0;
      else
      {
	if (NMS(magn, mag, *dx, *dy))	     /* check if passes NMS */
	  if (mag >= thresh2) {
	    *out = 1;			     /* definitely have an edge */

	    *(stktop++) = out;		     /* put edge pixel addr on stack */
	  }
	  else
	    *out = 2;			     /* maybe have an edge */
	else
	  *out = 0;			     /* no edge here */
      }
    }

    out  += 2;				     /* skip over last pixel on this line */
    dx   += 2;				     /* and first pixel on next line */
    dy   += 2;
    magn += 2;
  }

  while (stktop > stack) {
    out = *--stktop;

    /* look at neighbors. if a neighbor is 2 make it 1 and add to stack */
    /* continue until stack empty */

    if (*(out -= x_m + 1) == 2)              /* upper-left */
      *out = 1, *(stktop++) = out;
    if (*(++out) == 2)                       /* upper-middle */
      *out = 1, *(stktop++) = out;
    if (*(++out) == 2)                       /* upper-right */
      *out = 1, *(stktop++) = out;
    if (*(out += x_m) == 2)                  /* middle-right */
      *out = 1, *(stktop++) = out;
    if (*(out -= 2) == 2)                    /* middle-left */
      *out = 1, *(stktop++) = out;
    if (*(out += x_m) == 2)                  /* lower-left */
      *out = 1, *(stktop++) = out;
    if (*(++out) == 2)                       /* lower-middle */
      *out = 1, *(stktop++) = out;
    if (*(++out) == 2)                       /* lower-right */
      *out = 1, *(stktop++) = out;
  }

  { register uchar *outend = output + x_m*y_m; /* get rid of any remaining 2's */
    
    for (out = output; out < outend; out++ )
      if (*out == 2)
	*out = 0;
  }

  free(stack);
  
  return 0;
}

