#include <math.h>
#include <stdio.h>

/* scales angles in radians to fit within the 0-255 range of unsigned
   char variables */
#define ORIENT_SCALE 50.0
#define ORIENT_BASE  50
#define SEARCH_DIST  10
#define DIST_WEIGHT  2
#define ANGLE_WEIGHT 0.2

/* predeclare gaussian function */
double gaussian();

struct header {
  int rows;
  int cols;
};

typedef struct {
  int x, y;
} POINT2D;

typedef struct {
  int npoint;
  int back, foward;
  int x, y;
  float orient;
  float b;
  POINT2D *plist;
}  ENDPOINTINFO;

/* Neighborhood system */
POINT2D Neighbor_Table[] = {
  { 1,  0},  /* 0 */
  { 1,  1}, /* 1 */
  { 0,  1},  /* 2 */
  {-1,  1}, /* 3 */
  {-1,  0},   /* 4 */
  {-1, -1},   /* 5 */
  { 0, -1},   /* 6 */
  { 1, -1},  /* 7 */
  { 0,  0}    /* 8 */
};


char Progname[]="canny";
void ImposeEdges(unsigned char *origImg, unsigned char *edgImg,
		 struct header *hd, char *fname);
void  Local2SubPixel(struct header *hd, 
		     unsigned char *data, /* */
		     unsigned char *SaveData,
		     unsigned char *origImag,
		     unsigned char *magnitude, 
		     unsigned char *SaveOrg,
		     unsigned char *orientation,
		     int OperFlag);
void Extract_EndPoints(struct header *hd, 
		       unsigned char *data,
		       unsigned char *orient);
void DrawALine(int x1, int y1, int x2, int y2,
	       unsigned char *data,
	       unsigned char *orient,
	       struct header *hd);
int CheckALine(int x1, int y1, int x2, int y2,
	       unsigned char *data,
	       unsigned char *orient,
	       struct header *hd);
int overflowed;
float maxGrad;
float gcScale;
int filter_width;               /* length of 1-D gaussian mask */

int non_max_flag;
int main(argc,argv)
     int argc;
     char *argv[];
{
  int i,j,k,n;
  double s=1.0;            /* standard deviation of gaussian */
  long picsize;
  struct header hd,shd,ohd;       /* HIPS headers */
  int low=1,high=255;        /* tracker hysteresis parameters */
  char strength_flag=0,orientation_flag=0;    /* output flags */
  char magnitude_file[256],orientation_file[256];    /* i/o file names */
  char inFile[256], outFile[256], imposeFile[256];
  FILE *inFp, *outFp;
  FILE *tmpFp;
  FILE * sfd, *ofd;                    /*i/o file descriptors*/
  char predigested_ip_flag=0;            /* input flag */
  unsigned char *data;    /* input and output array */
  /*mag of del G before non-maximum suppression*/
  unsigned char *derivative_mag; 
  /*mag & orient after non-max sppression*/
  unsigned char *magnitude,*orientation;
  unsigned char *origImg;
  unsigned char *SaveData;
  unsigned char *SaveOrg;
  int imposeFlag =0;
  /* Sub pixels: always one */
  int subpixel_flag =0;
  
  overflowed = 0;
  maxGrad = 0.;
  gcScale = 20.;
  /* Initiallly a null string */
  inFile[0]='\0';
  outFile[0]='\0';
  non_max_flag =0;
  /* read args */
  for (i=1;i<argc;i++) {
    if (argv[i][0]=='-') {
      
      switch (argv[i][1]) {
      case 'l': /* track threshold lower limit */
	sscanf(argv[++i],"%d",&low);
	if (low<1 || low>255 || low>high) {
	  fprintf(stderr,"canny: lower threshold out of range\n");
	  exit(0);
	}
	break;
      case 'h': /* track threshold upper limit */
	sscanf(argv[++i],"%d",&high);
	if (high<0 || high>255 || high<low) {
	  fprintf(stderr,"canny: upper threshold out of range\n");
	  exit(0);
	}
	break;
      case 'd': /* arg is s.d. of gaussian */
	s=atof(argv[++i]);
	if (s<0.0) {
	  fprintf(stderr,"canny: standard deviation out of range\n");
	  exit(0);
	}
	break;
      case 'g':
	gcScale =atof(argv[++i]);
	if (gcScale <0.0001 || gcScale >1000000.) {
	  gcScale = 20.;
	  printf(" G is set to %6.4f\n", gcScale);
	}
	break;
      case 'm':
	non_max_flag = 1;
	break;
      case 's': /* dump strength data to a file */
	if (predigested_ip_flag) {
	  fprintf(stderr,"canny: -s flag incompatible with -p flag\n");
	  exit(0);
	}
	sscanf(argv[++i],"%s",magnitude_file);
	if ((sfd=fopen(magnitude_file,"w"))==0) {
	  fprintf(stderr,"canny: can't open %s\n",magnitude_file);
	  exit(0);
	}
	strength_flag=1;
	break;
      case 'o': /* dump orientation data to a file */
	if (predigested_ip_flag) {
	  fprintf(stderr,"canny: -o flag incompatible with -p flag\n");
	  exit(0);
	}
	sscanf(argv[++i],"%s",orientation_file);
	if ((ofd=fopen(orientation_file,"w"))==0) {
	  fprintf(stderr,"canny: can't open %s\n",orientation_file);
	  exit(0);
	}
	orientation_flag=1;
	break;
      case 'i':
	sscanf(argv[++i],"%s",imposeFile);
	imposeFlag =1;
	break;
      case 'u':
	subpixel_flag =1;
	break;
      case 'p': /* get i/p from strength & orient files */
	if (strength_flag || orientation_flag) {
	  fprintf(stderr,"canny: -p flag ");
	  fprintf(stderr,"incompatible with -s and -o flags\n");
	  exit(0);
	}
	predigested_ip_flag=1;
	sscanf(argv[++i],"%s",magnitude_file);
	sscanf(argv[++i],"%s",orientation_file);
	if ((sfd=fopen(magnitude_file,"r"))==0) {
	  fprintf(stderr,"canny: can't open %s\n",magnitude_file);
	  exit(0);
	}
	if ((ofd=fopen(orientation_file,"r"))==0) {
	  fprintf(stderr,"canny: can't open %s\n",orientation_file);
	  exit(0);
	}
	break;
      default:
	fprintf(stderr,"canny: unknown option -%c\n",argv[i][1]);
	exit(1);
	break;
      }
    }
    else {
      /* Input and Output file names */
      if (strlen(inFile)<1) {
	strcpy(inFile, argv[i]);
      }
      else {
	if (strlen(outFile)<1) {
	  strcpy(outFile,argv[i]);
	}
	else {
	  fprintf(stderr,"Canny: argument '%s' ignored.\n",argv[i]);
	}
      }
    }
  }
  
  if (strlen(inFile)<1) {
    inFp = stdin;
  } 
  else {
    inFp = fopen(inFile,"rb");
  }
  if (strlen(outFile)<1) {
    outFp = stdout;
  }
  else {
    outFp = fopen(outFile,"wb");
  }
  
  if (predigested_ip_flag) {
    fread_header(sfd,&shd);
    fread_header(ofd,&ohd);
    if (shd.cols != ohd.cols || shd.rows != ohd.rows) {
      fprintf(stderr,"canny: files must be the same size\n");
      exit(0);
    }
    hd=shd;
  }
  else {
    fread_header(inFp, &hd);
  }
  picsize=hd.cols*hd.rows;
  
  fwrite_header(outFp, &hd, (int)1);
  if (strength_flag) {
    shd=hd;
    fwrite_header(sfd,&shd,(int)1);
  }
  if (orientation_flag) {
    ohd=hd;
    fwrite_header(ofd,&ohd,(int)1);
  }
  
  /* allocate i/o array */
  if ((data=(unsigned char *)malloc(picsize))==(unsigned char *)NULL) {
    fprintf(stderr,"canny: can't alloc data\n");
    exit(0);
  }
  if ((derivative_mag=(unsigned char *)calloc(picsize,1))==
      (unsigned char *)NULL) {
    fprintf(stderr,"canny: can't alloc derivative_mag\n");
    exit(0);
  }
  if ((magnitude=(unsigned char *)calloc(picsize,1))
      ==(unsigned char *)NULL) {
    fprintf(stderr,"canny: can't alloc magnitude\n");
    exit(0);
  }
  if ((orientation=(unsigned char *)calloc(picsize,1))
      ==(unsigned char *)NULL) {
    fprintf(stderr,"canny: can't alloc orientation\n");
    exit(0);
  }
  
  if (predigested_ip_flag) {   
    /* read in strength and direction data, then go straight
       on to edge tracking */
    fprintf(stderr,"canny: reading predigested i/p data\n");
    if (fread(magnitude,picsize,1,sfd)!=1) {
      fprintf(stderr,"canny: incomplete strength file\n");
      exit(0);
    }
    if (fread(orientation,picsize,1,ofd)!=1) {
      fprintf(stderr,"canny: incomplete orientation file\n");
      exit(0);
    }
  }
  else {  
    /* no predigested i/p - `fraid we've got to calculate
       strength and direction data from scratch */
    /* read a frame from stdin */
    fread(data,picsize,1,inFp);
    
    if ((origImg=(unsigned char *)malloc(picsize))==(unsigned char *)NULL) {
      fprintf(stderr,"canny: can't alloc origImgdata\n");
      exit(0);
    }
    memcpy(origImg, data,picsize);
    /* call canny core routines - these perform a gaussian
       smoothing, calculate a gradient array and suppress
       non- maximal points in this array */
    canny_core(s,hd.cols,hd.rows,data,derivative_mag,magnitude,orientation);
  }
  
  /* optionally write out strength and orientation info */
  if (strength_flag) fwrite(magnitude,picsize,1,sfd);
  if (orientation_flag) fwrite(orientation,picsize,1,ofd);
  
  fprintf(stderr,"canny: about to track edges\n");
  /* track edges */
  thresholding_tracker(high,low,hd.cols,hd.rows,data,magnitude,orientation);
  /* Localization to sub pixel accuracies */
  SaveData = (unsigned char *)malloc(sizeof(unsigned char)*4*picsize);
  SaveOrg =  (unsigned char *)malloc(sizeof(unsigned char)*4*picsize);
  if (SaveOrg==NULL) {
    fprintf(stderr,"Cannot allocate memory for SaveOrg.\n");
    exit(1);
  }

  Local2SubPixel(&hd, SaveData, data, 
		 origImg, magnitude, 
		 SaveOrg, orientation, 
		 subpixel_flag);
  
  /* write tracked edges to standard output */
  fwrite(data,picsize,1,outFp);
  free(data);
  if (imposeFlag) {
    ImposeEdges(origImg, SaveData, &hd, imposeFile);
  }
  /*Extract_EndPoints(&hd,SaveData,SaveOrg);*/
  tmpFp = fopen("orientation.canny","wb");
  fwrite_header(tmpFp,&hd,(int)2);
  fwrite(SaveOrg,4*picsize, 1, tmpFp);
  fclose(tmpFp);

  tmpFp = fopen("magnitude.canny","wb");
  fwrite_header(tmpFp,&hd,(int)2);
  fwrite(SaveData,4*picsize, 1, tmpFp);
  fclose(tmpFp);

  free(SaveData);
  free(SaveOrg);
  free(magnitude);
  free(orientation);
  if (imposeFlag) {
    free(origImg);
  }
  fprintf(stderr,"Number of overflowed edge: %d Max Grad: %6.2f\n",
	  overflowed, maxGrad);
  fclose(inFp);
  fclose(outFp);
  if (predigested_ip_flag) {
    fclose(sfd);
    fclose(ofd);
  }
  if (strength_flag) {
    fclose(sfd);
  }
  if (orientation_flag) {
    fclose(ofd);
  }
  return 0;
  
}

/*            canny_core.c                */

/* core routine for the canny family of programs */


/* hypot can't cope when both it's args are zero, hence this hack.... */
double hypotenuse(x,y)
double x,y;
{
    if (x==0.0 && y==0.0) return(0.0);
    else return(hypot(x,y));
}

canny_core(s,cols,rows,data,derivative_mag,magnitude,orientation)
     double s;            /* standard deviation */
     int cols,rows;            /* picture dimensions */
     unsigned char *data,*derivative_mag,*magnitude,*orientation;
{
  float *gsmooth_x,*gsmooth_y;
  float *derivative_x,*derivative_y;
  int i,j,k,n;            /* counters */
  int t;                /* temp. grad magnitude variable */
  double a,b,c,d,g0,g1;        /* mask generation intermediate vars*/
  double ux,uy;
  double t1,t2;
  double G[20],dG[20],D2G[20];    /*gaussian & derivative filter masks*/
  double gc,gn,gs,gw,ge,gnw,gne,gsw,gse;
  int picsize,jstart,jlimit;
  int ilimit;
  register jfactor;
  int kfactor1,kfactor2;
  int kfactor;
  register cindex,nindex,sindex,windex,eindex,nwindex,neindex,swindex,seindex;
  int low=1,high=255;        /* tracker hysteresis parameters */
  int cols_plus,cols_minus;    /*cols+1 and cols-1 respectively*/
  /* used to measure how oft mag array overflows */ 
  int mag_overflow_count=0; 
  /* used to measure how oft mag array underflows */
  int mag_underflow_count=0; 
  
  picsize=cols*rows;        /* picture area */

  /* calc coeffs for 1-dimensional G, dG/dn and
     Delta-squared G filters */
  for(n=0; n<20; ++n) {
    a=gaussian(((double) n),s);
    if (a>0.005 || n<2) {
      b=gaussian((double)n-0.5,s);
      c=gaussian((double)n+0.5,s);
      d=gaussian((double)n,s*0.5);
      fprintf(stderr,"a,b,c: %lf,%lf,%lf\n",a,b,c);
      G[n]=(a+b+c)/3/(6.283185*s*s);
      dG[n] = b-c;
      D2G[n]=1.6*d-a; /* DOG */
      fprintf(stderr,"G[%d]: %lf\n",n,G[n]);
      fprintf(stderr,"dG[%d]: %lf\n",n,dG[n]);
      fprintf(stderr,"D2G[%d]: %lf\n",n,D2G[n]);
      
    }
    else break;
  }
  filter_width=n;
  
  fprintf(stderr,"canny_core: smooth pic\n");
  /* allocate space for gaussian smoothing arrays */
  if ((gsmooth_x=(float *)calloc(picsize,sizeof(float)))==(float *)NULL) {
    fprintf(stderr,"can't alloc gsmooth_x\n");
    exit(0);
  }
  if ((gsmooth_y=(float *)calloc(picsize,sizeof(float)))==(float *)NULL) {
    fprintf(stderr,"can't alloc gsmooth_y\n");
    exit(0);
  }
  
  /* produce x- and y- convolutions with gaussian */
  
  ilimit=cols-(filter_width-1);
  jstart=cols*(filter_width-1);
  jlimit=cols*(rows-(filter_width-1));
  for (i=filter_width-1;i<ilimit;++i)  {
    for(jfactor=jstart; jfactor<jlimit; jfactor+=cols) {
      cindex=i+jfactor;
      t1=data[cindex]*G[0];
      t2=t1;
      for(k=1,kfactor1=cindex-cols, kfactor2=cindex+cols;
	  k<filter_width;
	  k++, kfactor1-=cols, kfactor2+=cols) {
	t1 +=G[k]*(data[cindex-k]+
		  data[cindex+k]);
	t2 +=G[k]*(data[kfactor1]+
		   data[kfactor2]);
      }
      /* Note: t1 is the smoothing along row: x */
      /*       t2 is the smoothing along column: y */
      gsmooth_x[cindex]=t1;
      gsmooth_y[cindex]=t2;
    }
  }
  
  /* allocate space for gradient arrays */
  fprintf(stderr,"canny_core: find grad\n");
  if ((derivative_x=(float *)calloc(picsize,sizeof(float)))==(float *)NULL) {
    fprintf(stderr,"can't alloc x\n");
    exit(0);
  }
  /* produce x and y convolutions with derivative of
     gaussian */
  
  for (i=filter_width-1;i<ilimit;++i) {
    for(jfactor=jstart; jfactor<jlimit; jfactor+=cols) {
      t1=0;
      cindex=i+jfactor;
      for (k=1;k<filter_width;++k) {
	t1+=dG[k]*(gsmooth_y[cindex-k]-
		   gsmooth_y[cindex+k]);
      }
      derivative_x[cindex]=t1;
    }
  }
  free(gsmooth_y);
  if ((derivative_y=(float *)calloc(picsize,sizeof(float)))==(float *)NULL) {
    fprintf(stderr,"can't alloc y\n");
    exit(0);
  }
  
  for (i=n;i<cols-n;++i) {
    for(jfactor=jstart;jfactor<jlimit;jfactor+=cols) {
      t2=0;
      cindex=i+jfactor;
      for (k=1,kfactor=cols; k<filter_width; k++,kfactor+=cols) {
	t2+=dG[k]*(gsmooth_x[cindex-kfactor]-gsmooth_x[cindex+kfactor]);
      }
      derivative_y[cindex]=t2;
    }
  }
  free(gsmooth_x);
  
  /* non-maximum suppression (4 cases for orientation of line
     of max slope) */
  
  fprintf(stderr,"canny_core: non-maximum suppression\n");
  ilimit=cols-filter_width;
  jstart=cols*filter_width;
  jlimit=cols*(rows-filter_width);
  
  for (i=filter_width;i<ilimit;++i) {
    for (jfactor=jstart; jfactor<jlimit; jfactor+=cols) {
      /* calculate current indeces */
      cindex=i+jfactor;
      nindex=cindex-cols;
      sindex=cindex+cols;
      windex=cindex-1;
      eindex=cindex+1;
      nwindex=nindex-1;
      neindex=nindex+1;
      swindex=sindex-1;
      seindex=sindex+1;
      ux=derivative_x[cindex];
      uy=derivative_y[cindex];
      gc=hypotenuse(ux,uy);
      /* scale gc to fit into an unsigned char array */
      t=gc*gcScale;
      if (t >= 256) {
	overflowed++;
	if (t > maxGrad) {
	  maxGrad = t;
	}
      }
      derivative_mag[cindex]=(t<256 ? t : 255);
      if (non_max_flag == 0) {
	gn=hypotenuse(derivative_x[nindex],derivative_y[nindex]);
	gs=hypotenuse(derivative_x[sindex],derivative_y[sindex]);
	gw=hypotenuse(derivative_x[windex],derivative_y[windex]);
	ge=hypotenuse(derivative_x[eindex],derivative_y[eindex]);
	gne=hypotenuse(derivative_x[neindex],derivative_y[neindex]);
	gse=hypotenuse(derivative_x[seindex],derivative_y[seindex]);
	gsw=hypotenuse(derivative_x[swindex],derivative_y[swindex]);
	gnw=hypotenuse(derivative_x[nwindex],derivative_y[nwindex]);
	
	if (ux*uy>0) {
	  if(fabs(ux)<fabs(uy)) {
	    if((g0=fabs(uy*gc))
	       < fabs(ux*gse+(uy-ux)*gs) ||
	       g0<=fabs(ux*gnw+(uy-ux)*gn))
	      continue;
	  }
	  else  {
	    if((g0=fabs(ux*gc))
	       < fabs(uy*gse+(ux-uy)*ge) ||
	       g0<=fabs(uy*gnw+(ux-uy)*gw))
	      continue;
	  }
	}
	else {
	  if(fabs(ux)<fabs(uy))  {
	    if((g0=fabs(uy*gc))
	       < fabs(ux*gne-(uy+ux)*gn) ||
	       g0<=fabs(ux*gsw-(uy+ux)*gs))
	      continue;
	  }
	  else  {
	    if((g0=fabs(ux*gc))
	       < fabs(uy*gne-(ux+uy)*ge) ||
	       g0<=fabs(uy*gsw-(ux+uy)*gw))
	      continue;
	  }
	}
      }
      /* seems to be a good scale factor */
      magnitude[cindex]=derivative_mag[cindex];
      /* pi*40 ~= 128 - direction is (thought
	 of as) a signed byte */
      if (gc<0.001) {
	g1 = -1;
      }
      else {
	g1 = atan2(uy, ux)+M_PI/2.;
	if (g1<0.) g1 += M_PI;
	if (g1 >= M_PI)
	  g1 -= M_PI;
      }
      orientation[cindex]=(unsigned char)(g1*ORIENT_SCALE+ORIENT_BASE);
    }
  } 
  
  free(derivative_x);
  free(derivative_y);
}

/*            canny_subr.c                */

/* Subroutines used by *canny progs (but not by *canny_j progs) */

/* gaussian function (centred at the origin and ignoring the factor
   of 1/(s*sqrt(2*PI)) ) */
double gaussian(x,s)
double x,s;
{
    return(exp((-x*x)/(2*s*s)));
}

/* track the edges in the magnitude array, starting at points that exceed
   the "high" threshold, and continuing to track as long as point values
   don't duck below the "low" threshold (yes, it's hysteresis...I'm sure
   that hyster means "womb" (as in hysterical), but I don't know what
   it's doing in a common engineering term) */
thresholding_tracker(high,low,cols,rows,data,magnitude,orientation)
int high,low;        /* threshold values */
int cols,rows;        /* picture size */
unsigned char *data;    /* o/p pic array */
unsigned char *magnitude;    /* gradient magnitude array */
unsigned char *orientation;    /* gradient direction array */
{
    int i,j,k;    /* counters */
    int picsize;    /* picture area */

fprintf(stderr,"thresholding_tracker: tracking edges, high=%d, low=%d\n",high,low);
    /* clear data array before tracking */
    picsize=cols*rows;
    for (i=0;i<picsize;++i) data[i]=0;

    /* threshold image with hysteresis: follow from
       each strong edge point */
    for (i=0;i<cols;++i)
    {
        for (j=0;j<rows;++j)
            if (magnitude[i+cols*j]>=high)
                follow(i,j,low,cols,rows,data,magnitude,orientation);
    }
}

/* follow a connected edge */
follow(i,j,low,cols,rows,data,magnitude,orientation)
int i,j;    /* start point */
int low;    /* lower threshold value */
int cols,rows;    /* picture dimensions */
unsigned char *data;    /* o/p pic array */
unsigned char *magnitude;    /* gradient magnitude array */
unsigned char *orientation;    /* gradient direction array */
{
    int k,l;        /* counters */
    int i_plus_1,i_minus_1,j_plus_1,j_minus_1;
    long index,kindex;
    char break_flag;

    i_plus_1=i+1;
    i_minus_1=i-1;
    j_plus_1=j+1;
    j_minus_1=j-1;
    index=i+j*cols;
    if (j_plus_1>=rows) j_plus_1=rows-1;
    if (j_minus_1<0) j_minus_1=0;
    if (i_plus_1>=cols) i_plus_1=cols-1;
    if (i_minus_1<0) i_minus_1=0;

    if (!data[index])
    {
      /*fprintf(stderr,"following %d %d\n",i,j);*/
      /* current point must be added to the list of tracked points */
      data[index]=magnitude[index];
      /* now we can check immediately adjacent points to see if
	 they too could be added to the track */
      break_flag=0;
      for (k=i_minus_1;k<=i_plus_1;k++)
        {
	  for(l=j_minus_1;l<=j_plus_1;++l)
            {
	      kindex=k+l*cols;
	      if (!(l==j && k==i) &&
		  magnitude[kindex]>=low &&
		  ( abs(orientation[index]-orientation[kindex]) <
		    ((int)(M_PI*ORIENT_SCALE/4.)) ||
		  abs(orientation[index]-orientation[kindex]) >
		  ((int)(M_PI*3.*ORIENT_SCALE/4.))) )
		  
                {
		  if (follow(k,l,low,cols,rows,data,magnitude,orientation))
                    {
		      break_flag=1;
		      break;
                    }
                }
            }
	  if (break_flag) break;
        }
      return(1);
    }
    else return(0);
}


int getline_aux (FILE *file, char *buffer, unsigned int n)
{
  int reading = 0;
  while (!reading) {
    if (!fgets (buffer, n, file))
      return 0;
    reading = (buffer [0] != '#');
  }
  return 1;
}

int fread_header(FILE * fp, struct header * hd)
{
  int maxgray;
  char buf2[3];
  char buf[256];
  
  if (!getline_aux (fp, buf, 256) || (sscanf(buf, "%s", buf2) != 1))
    error("fread_header: first line");
  
  if (strcmp(buf2,"P5") !=0)
    error("fread_header: Not a raw PGM file");

  if (!getline_aux (fp, buf, 256) || 
      (sscanf(buf, "%d %d", &(hd->cols),&(hd->rows)) != 2)) {
    error("fread_header: couldn't read X Y");
  }
  if (!getline_aux (fp, buf, 256) || 
      (sscanf(buf, "%d", &maxgray) != 1)) {
    error("fread_header: couldn't read maxgray");
  }
  if (maxgray > 255) {
    error("fread_header: maxgray > 255");
  }
  return 1;
}

error(char *msg)
{
  fprintf(stderr, "canny: error in %s\n",msg);
  exit(-1);
}

int fwrite_header(FILE * fp, struct header * hd, int factor)
{
  fprintf(fp,"%s\n","P5");
  fprintf(fp,"%d %d\n", hd->cols*factor, hd->rows*factor);
  fprintf(fp,"%d\n",255);
}

void ImposeEdges(unsigned char *origImg, unsigned char *edgImg,
		 struct header *hd, char *fname)
{
  FILE *fp;
  unsigned char *lineBuf1, *lineBuf2;
  int i,j,k,k1,m;
  lineBuf1 = (unsigned char *)malloc(sizeof(unsigned char)*2*3*hd->cols);
  lineBuf2 = (unsigned char *)malloc(sizeof(unsigned char)*2*3*hd->cols);
  for (i=0; i<(6*hd->cols); i++) {
    lineBuf1[i]=0;
    lineBuf2[i]=0;
  }
  fp = fopen(fname,"wb");
  fprintf(fp,"%s\n","P6");
  fprintf(fp,"%d %d\n", 2*hd->cols, 2*hd->rows);
  fprintf(fp,"%d\n",255);
  for (i=0; i<hd->rows; i++) {
    for (k= (i*hd->cols),k1=(2*i*2*hd->cols), j=0; j<hd->cols; 
	 k++,k1+=2,j++) {
      for (m=0; m<6; m++) {
	lineBuf1[6*j+m] = origImg[k];
	lineBuf2[6*j+m] = origImg[k];
      }
      /*if (edgImg[k1] !=0) {
	printf("Edge at %d %d is not zero.\n", j,i);
      }
      */
      for (m=0; m<2; m++) {
	if (edgImg[k1+m]!=0) {
	  lineBuf1[6*j+3*m] = 255;
	  lineBuf1[6*j+3*m+1] = lineBuf1[6*j+3*m+2] = 0;
	}
      }
      
      for (m=0; m<2; m++) {
	if (edgImg[k1+hd->cols*2+m] !=0) {
	  lineBuf2[6*j+3*m] = 255;
	  lineBuf2[6*j+3*m+1] = lineBuf2[6*j+3*m+2] = 0;
	}
      }
      
    }
    fwrite(lineBuf1, sizeof(unsigned char), 6*hd->cols,fp);
    fwrite(lineBuf2, sizeof(unsigned char), 6*hd->cols,fp);
  }
  
  fclose(fp);
  fp = fopen("impose.tmp","wb");
  fwrite_header(fp,hd,(int)2);
  fwrite(edgImg,4*hd->cols*hd->rows,1,fp);
  fclose(fp);
  free(lineBuf1);
  free(lineBuf2);
}

void  Local2SubPixel(struct header *hd, 
		     unsigned char *data, /* */
		     unsigned char *SaveData, /* */
		     unsigned char *origImag,
		     unsigned char *magnitude, 
		     unsigned char *FinalOr,
		     unsigned char *orientation,
		     int OperFlag)
{
  int i,j,k;
  int wsize;
  int picsize;
  int cindex;
  int dy, dx;
  int direct;
  int maxdiff;
  int maxindex;
  int diff;
  int fpindex,pindex;
  int nowindex;
  int preindex;
  int dorow, docol;
  float *weight;
  float edgedir;
  picsize=hd->cols * hd->rows;        /* picture area */
  /*  reset to zeros first */
  for (i=0; i<(4*picsize); i++) {
    data[i]=0;
    FinalOr[i]=0;
  }

  wsize = filter_width;
  weight = (float *)malloc(sizeof(float)*(wsize+1));
  weight[0]=weight[1]=1.0;
  for (i=2; i<=wsize; i++)
    weight[i] = 1.0/((float)i+.5);
  for (i=wsize; i<(hd->rows-wsize); i++) {
    for(j=wsize; j<(hd->cols-wsize); j++) {
      cindex = i*hd->cols+j;
      if (SaveData[cindex] !=0) {
	direct = (orientation[cindex]-ORIENT_BASE) +
	  ((int)(M_PI*ORIENT_SCALE/8.));
	direct /= ( (int)(M_PI*ORIENT_SCALE/4.));
	/*if (direct !=0 && direct !=2 && direct != 4) 
	  printf("Orien: %d direct: %d\n",orientation[cindex],direct);
	  */
	if (direct>=4) direct = 0;
	if (direct<0) {
	  fprintf(stderr,"Error in orientation map at %d,%d with value %d\n",
		  i,j,orientation[cindex]);
	  direct = 0;
	}
	switch(direct) {
	case 0:
	  dy =1; dx =0;
	  break;
	case 1:
	  dy = -1;
	  dx = 1;
	  break;
	case 2:
	  dx =1;
	  dy =0;
	  break;
	case 3:
	  dx = 1;
	  dy = 1;
	  break;
	}
	preindex = cindex;
	/* Positive direction first */
	maxdiff = -1;
	for (k=1; k<wsize; k++) {
	  nowindex = preindex + dy * hd->cols + dx;
	  
	  diff = abs(origImag[nowindex] - origImag[preindex])*weight[k];
	  if (diff > maxdiff) {
	    maxindex = 2*k-1;
	    maxdiff = diff;
	  }
	  preindex = nowindex;
	}
	/* The other way */
	preindex = cindex;
	for (k=1; k<wsize; k++) {
	  nowindex = preindex - dy * hd->cols - dx;
	  diff = abs(origImag[nowindex] - origImag[preindex])*weight[k];
	  if (diff > maxdiff) {
	    maxindex = 1-2*k;
	    maxdiff = diff;
	  }
	  preindex = nowindex; 
	}
	/* Assign the value in the original image */
	fpindex = 2*i*2*hd->cols + 2 *j;
	if (OperFlag==0) {
	  fpindex += 2*hd->cols + 1;
	}
	else {
	  fpindex += maxindex * dy * 2*hd->cols + maxindex * dx;
	}
	data[fpindex] = SaveData[cindex];
	FinalOr[fpindex] = orientation[cindex];
      }
    }
  }
  
  for (i=wsize; i<(hd->rows-wsize); i++) {
    for(j=wsize; j<(hd->cols-wsize); j++) {
      cindex = 2*i*2*hd->cols+2*j;
      for (k=1; k<=1; k++) {
	
	switch(k) {
	case 0:
	  pindex = cindex+1;
	  break;
	case 1:
	  pindex = cindex+2*hd->cols+1;
	  break;
	case 2:
	  pindex = cindex+2*hd->cols;
	  break;
	}
	if (data[pindex] !=0) {
	  /* Check horizontally */
	  if (data[pindex+1] ==0 && data[pindex+2] !=0) {
	    data[pindex+1] = (data[pindex]+data[pindex+2])/2;
	    FinalOr[pindex+1] =  (FinalOr[pindex]+FinalOr[pindex+2])/2;
	  }
	  /* Check vertically */
	  if (data[pindex+2*2*hd->cols] !=0 && data[pindex+2*hd->cols]==0 ) {
	    data[pindex+2*hd->cols] = (data[pindex]+
				       data[pindex+2*2*hd->cols])/2;
	    FinalOr[pindex+2*hd->cols] = (FinalOr[pindex]+
					  FinalOr[pindex+2*2*hd->cols])/2;
	  }
	  
	}
      }
    }
  }

  for (i=wsize; i<(hd->rows-wsize); i++) {
    for(j=wsize; j<(hd->cols-wsize); j++) {
      cindex = 2*i*2*hd->cols+2*j;
      for (k=0; k<=3; k++) {
	dorow = 2*i;
	docol = 2*j;
	
	switch(k) {
	case 0:
	  pindex = cindex+1;
	  docol++;
	  break;
	case 1:
	  pindex = cindex+2*hd->cols+1;
	  dorow++;
	  docol++;
	  break;
	case 2:
	  pindex = cindex+2*hd->cols;
	  dorow++;
	  break;
	}
	if (data[pindex] !=0) {
	  
	  /* Check diagnoally */
	  edgedir = ((float)(FinalOr[pindex]-ORIENT_BASE))/ORIENT_SCALE;
	  if (data[pindex+2*hd->cols]==0 &&
	      data[pindex+1]==0 &&
	      fabs(edgedir-(M_PI/4.)) < (M_PI/4.) &&
	      data[pindex+2+2*2*hd->cols]!=0) {
	    
	    data[pindex+2*hd->cols+1]=(data[pindex]+
				       data[pindex+2+2*2*hd->cols])/2;
	    FinalOr[pindex+2*hd->cols+1]=(FinalOr[pindex]+
					  FinalOr[pindex+2+2*2*hd->cols])/2;
	  }
	  if (data[pindex+2*hd->cols]==0 &&
	      data[pindex-1]==0 &&
	      fabs(edgedir-(M_PI*3.0/4.0)) < (M_PI/4.0) &&
	      data[pindex-2+2*2*hd->cols]!=0) {
	    
	    data[pindex-1+2*hd->cols]=(data[pindex]+
				       data[pindex-2+2*2*hd->cols])/2;
	    FinalOr[pindex-1+2*hd->cols]=(FinalOr[pindex]+
					  FinalOr[pindex-2+2*2*hd->cols])/2;
	  }
	}
      }
    }
  }
  
  free(weight);
  return;
}

void Extract_EndPoints(struct header *hd, 
		       unsigned char *data,
		       unsigned char *orient)
{
  int i,j,k;
  int edge_dir;
  int pindex;
  int cindex;
  int cdir;
  int back_flag, foward_flag;
  float direct;
  int jstart, jend;
  int istart, iend;
  int i1,j1;
  int bestdir;
  ENDPOINTINFO  *bestend;
  float dist;
  float bestdist;
  ENDPOINTINFO  **endpoints;
  endpoints = (ENDPOINTINFO **)malloc(sizeof(ENDPOINTINFO *)*4*
				      hd->rows*hd->cols);
  for (i=0; i<(2*hd->rows*2*hd->cols);i++)
    endpoints[i] = NULL;
  for (i=0; i<(2*hd->rows); i++) {
    for (j=0; j<(2*hd->cols); j++) {
      pindex = i*2*hd->cols+j;
      if (data[pindex]!=0) {
	direct = (float)(orient[pindex]-ORIENT_BASE) +
	  (M_PI*ORIENT_SCALE/8.);
	direct /= (M_PI*ORIENT_SCALE/4.);
	edge_dir = (int)direct;
	if (edge_dir>=4) edge_dir = 0;
	/* Check if it is an end point */
	back_flag = 0;
	foward_flag = -1;
	for (k=0; k<8; k++) {
	  cdir = (edge_dir +k +8)%8;
	  cindex = pindex + Neighbor_Table[cdir].x + 
	     Neighbor_Table[cdir].y * 2*hd->cols;
	  if (data[cindex] !=0) {
	    if (foward_flag == -1) {
	      foward_flag = k;
	    }
	    else {
	      if ( (k-foward_flag)>1 && (8-k+foward_flag)>1) {
		back_flag =1;
		break;
	      }
	    }
	  }
	}
	foward_flag = 0;
	if (back_flag ==0) {
	  
	  for (k=(0-1); k<=1; k++) {
	    cdir = (edge_dir +k +8)%8;
	    cindex = pindex + Neighbor_Table[cdir].x + 
	      Neighbor_Table[cdir].y * 2*hd->cols;
	    if (data[cindex] !=0) {
	      foward_flag = 1;
	    }
	    /* Diagnoally */
	    cindex = pindex - Neighbor_Table[cdir].x - 
	      Neighbor_Table[cdir].y * 2*hd->cols;
	    if (data[cindex] !=0) {
	      back_flag = 1;
	    }
	  }
	  if (i==233 && j==259) {
	    printf("Orient: %d Edge dir: %d F: %d B: %d\n",
		   orient[pindex],
		   edge_dir, foward_flag,
		   back_flag);
	  }
	}
	else {
	  back_flag =1;
	  foward_flag =1;
	}
	if (back_flag ==0 || foward_flag ==0) {
	  endpoints[pindex] = (ENDPOINTINFO *)malloc(sizeof(ENDPOINTINFO));
	  endpoints[pindex]->x = j;
	  endpoints[pindex]->y = i;
	  endpoints[pindex]->orient = 
	    ((float)(orient[pindex]-ORIENT_BASE))*180./
	    (ORIENT_SCALE*M_PI);
	  endpoints[pindex]->back = (back_flag ==0);
	  endpoints[pindex]->foward = (foward_flag ==0);
	}
      }
    }
  }
  printf(" End point Checking:\n");
  while(1) {
    printf("Pixel to check: ");
    scanf("%d%d",&j,&i);
    if (j<0 || i<0) break;
    pindex = i*hd->cols*2+j;
    if (endpoints[pindex] == NULL) {
      printf(" %d,%d Not an end point.\n",j,i);
    }
    else {
      if (endpoints[pindex]->back) {
	printf("Back point: Orient %f\n",
	       endpoints[pindex]->orient+180.);
      }
      if (endpoints[pindex]->foward) {
	printf("Foward point: Orient %f\n",
	       endpoints[pindex]->orient);
      }
    }
  }
  
  for (i=0; i<(2*hd->rows); i++) {
    for (j=0; j<(2*hd->cols); j++) {
      pindex = i*2*hd->cols+j;
      if (endpoints[pindex]!=NULL) {
	if (endpoints[pindex]->back) {
	  direct = endpoints[pindex]->orient+180.;
	  edge_dir = (int)((direct+45)/90);
	  if (edge_dir>=4) edge_dir = 0;
	  switch(edge_dir) {
	  case 0:
	    jstart = 1;
	    jend = SEARCH_DIST*2;
	    istart = 0-SEARCH_DIST;
	    iend = SEARCH_DIST;
	    break;
	  case 1:
	    jstart = 0-SEARCH_DIST;
	    jend = SEARCH_DIST;
	    istart = 1;
	    iend = 2*SEARCH_DIST;
	    break;
	  case 2:
	    jstart = 0-2*SEARCH_DIST;
	    jend = -1;
	    istart =0 - SEARCH_DIST;
	    iend = SEARCH_DIST;
	    break;
	  case 3:
	    jstart = 0 - SEARCH_DIST;
	    jend = SEARCH_DIST;
	    istart = 0 - 2*SEARCH_DIST;
	    iend = -1;
	    break;	  
	  
	  }
	  if ( (i+iend)<0) {
	    iend = 1;
	    istart = 2;
	  }
	  else {
	    if ( (i+istart)>= 2*hd->rows) {
	      iend = -2;
	      istart = -1;
	    }
	    else {
	      if ( (i+iend)>=2*hd->rows) {
		iend = 2*hd->rows -1 -i;
	      }
	      if ( (i+istart)<0) {
		istart = 0-i;
	      }
	    }
	  }
	  
	  if ( (j+jend)<0) {
	    jend = 1;
	    jstart = 2;
	  }
	  else {
	    if ( (j+jstart)>= 2*hd->cols) {
	      jend = -2;
	      jstart = -1;
	    }
	    else {
	      if ( (j+jend)>=2*hd->cols) {
		jend = 2*hd->cols -1 -j;
	      }
	      if ( (j+jstart)<0) {
		jstart = 0-j;
	      }
	    }
	  }
	  
	  bestend = NULL;
	  bestdist = 10000;
	  for (i1=istart; i1<=iend; i1++) {
	    for (j1=jstart; j1<=jend; j1++) {
	      cindex = pindex + i1*2*hd->cols + j1;
	      if (endpoints[cindex]!= NULL) {
		if (endpoints[cindex]->foward!=0) {
		  dist = sqrt((double)((i-endpoints[cindex]->y)*
				       (i-endpoints[cindex]->y)+
				       (j-endpoints[cindex]->x)*
				       (j-endpoints[cindex]->x))) * 
		    DIST_WEIGHT + 
		    fabs(fabs(direct-endpoints[cindex]->orient)-180.) *
		    ANGLE_WEIGHT;
		  if (dist <bestdist && 
		      fabs(fabs(direct-endpoints[cindex]->orient)-180.)<90.) {
		    bestend =endpoints[cindex];
		    bestdir = 0;
		    bestdist = dist;
		  }
		}
		if (endpoints[cindex]->back !=0) {
		   dist = sqrt((double)((i-endpoints[cindex]->y)*
				       (i-endpoints[cindex]->y)+
				       (j-endpoints[cindex]->x)*
				       (j-endpoints[cindex]->x))) * 
		    DIST_WEIGHT + 
		    fabs(fabs(direct-(endpoints[cindex]->orient+180))-180.) *
		    ANGLE_WEIGHT;
		  if (dist <bestdist && 
		      fabs(fabs(direct-(endpoints[cindex]->orient+180))-180.)
		      <90.) {
		    bestdir = 1;
		    bestend =endpoints[cindex];
		    bestdist = dist;
		  }
		}
	      }
	    }
	  }
	  if (bestend != NULL &&  
	      ((bestdir==0 && 
		fabs(fabs(direct-bestend->orient)-180.)<60) ||
	       (bestdir == 1  &&
		fabs(fabs(direct-(bestend->orient+180))-180.)<60))) {
	    printf(" End point %d, %d (B) should be connect to %d,%d(F)\n",
		   j,i,bestend->x, bestend->y);
	    if (CheckALine(j,i,bestend->x, bestend->y,data,orient,hd)==0) {
	      DrawALine(j,i,bestend->x, bestend->y,data,orient,hd);
	      if (bestdir==0) 
		bestend->foward = 0;
	      else
		bestend->back = 0;
	    }
	  }
	  endpoints[pindex]->back =0;
	}
	if (endpoints[pindex]->foward) {
	  
	  direct = endpoints[pindex]->orient;
	  edge_dir = (int)((direct+45)/90);
	  if (edge_dir>=4) edge_dir = 0;
	  switch(edge_dir) {
	  case 0:
	    jstart = 1;
	    jend = SEARCH_DIST*2;
	    istart = 0-SEARCH_DIST;
	    iend = SEARCH_DIST;
	    break;
	  case 1:
	    jstart = 0-SEARCH_DIST;
	    jend = SEARCH_DIST;
	    istart = 1;
	    iend = 2*SEARCH_DIST;
	    break;
	  case 2:
	    jstart = 0-2*SEARCH_DIST;
	    jend = -1;
	    istart =0 - SEARCH_DIST;
	    iend = SEARCH_DIST;
	    break;
	  case 3:
	    jstart = 0 - SEARCH_DIST;
	    jend = SEARCH_DIST;
	    istart = 0 - 2*SEARCH_DIST;
	    iend = -1;
	    break;
	  }
	  	  if ( (i+iend)<0) {
	    iend = 1;
	    istart = 2;
	  }
	  else {
	    if ( (i+istart)>= 2*hd->rows) {
	      iend = -2;
	      istart = -1;
	    }
	    else {
	      if ( (i+iend)>=2*hd->rows) {
		iend = 2*hd->rows -1 -i;
	      }
	      if ( (i+istart)<0) {
		istart = 0-i;
	      }
	    }
	  }
	  
	  if ( (j+jend)<0) {
	    jend = 1;
	    jstart = 2;
	  }
	  else {
	    if ( (j+jstart)>= 2*hd->cols) {
	      jend = -2;
	      jstart = -1;
	    }
	    else {
	      if ( (j+jend)>=2*hd->cols) {
		jend = 2*hd->cols -1 -j;
	      }
	      if ( (j+jstart)<0) {
		jstart = 0-j;
	      }
	    }
	  }
	  
	  bestend = NULL;
	  bestdist = 1000;
	  for (i1=istart; i1<=iend; i1++) {
	    for (j1=jstart; j1<=jend; j1++) {
	      cindex = pindex + i1*2*hd->cols + j1;
	      if (endpoints[cindex]!= NULL) {
		if (endpoints[cindex]->foward!=0) {
		  dist = sqrt((double)((i-endpoints[cindex]->y)*
				       (i-endpoints[cindex]->y)+
				       (j-endpoints[cindex]->x)*
				       (j-endpoints[cindex]->x))) * 
		    DIST_WEIGHT + 
		    fabs(fabs(direct-endpoints[cindex]->orient)-180.) *
		    ANGLE_WEIGHT;
		  if (dist <bestdist && 
		      fabs(fabs(direct-endpoints[cindex]->orient)-180.)<90.) {
		    bestend =endpoints[cindex];
		    bestdir = 0;
		    bestdist = dist;
		  }
		}
		if (endpoints[cindex]->back !=0) {
		   dist = sqrt((double)((i-endpoints[cindex]->y)*
				       (i-endpoints[cindex]->y)+
				       (j-endpoints[cindex]->x)*
				       (j-endpoints[cindex]->x))) * 
		    DIST_WEIGHT + 
		    fabs(fabs(direct-(endpoints[cindex]->orient+180))-180.) *
		    ANGLE_WEIGHT;
		  if (dist <bestdist && 
		      fabs(fabs(direct-(endpoints[cindex]->orient+180))-180.)
		      <90.) {
		    bestdir = 1;
		    bestend =endpoints[cindex];
		    bestdist = dist;
		  }
		}
	      }
	    }
	  }
	  if (i==171 && j ==177) {
	    printf("Orient: %f Dir: %d\n", direct, edge_dir);
	    printf("I range: %d %d Jrnage: %d %d\n",
		   istart, iend, jstart, jend);
	    if (bestend != NULL) {
	      printf("Best: %d %d Bestdir: %d\n",
		     bestend->x, bestend->y,bestdir);
	    }
	    else {
	      printf("No point.\n");
	    }
	  }
	  if (bestend != NULL &&  
	      ((bestdir==0 && 
		fabs(fabs(direct-bestend->orient)-180.)<60) ||
	       (bestdir == 1  &&
		fabs(fabs(direct-(bestend->orient+180))-180.)<60))) {
	    
	    printf(" End point %d, %d (forward) should be connect to %d,%d(back)\n",
		   j,i,bestend->x, bestend->y);
	    if (CheckALine(j,i,bestend->x, bestend->y,data,orient,hd)==0) {
	      DrawALine(j,i,bestend->x, bestend->y,data,orient,hd);
	      if (bestdir==0) 
		bestend->foward =0;
	      else
		bestend->back = 0;
	    }
	  }
	  endpoints[pindex]->foward =0;
	}
      }
    }
  }
  for (i=0; i<(2*hd->rows*2*hd->cols);i++) {
    if (endpoints[i]  != NULL) {
      free(endpoints[i]);
    }
  }
  free(endpoints);
}

void DrawALine(int x1, int y1, int x2, int y2,
	       unsigned char *data,
	       unsigned char *orient,
	       struct header *hd)
{
  int i, j, k;
  int step;
  int mag;
  int or;
  int xx, yy;
  if (x1==x2 && y1==y2) return;
  mag = (data[x1+2*hd->cols*y1]+data[x2+2*hd->cols*y2])/2;
  or = (orient[x1+2*hd->cols*y1]+orient[x2+2*hd->cols*y2])/2;
  if (abs(x2-x1)>=abs(y2-y1)) {
    step = 1;
    if (x1>x2) step = -1;
    xx = x1+step;
    while(xx != x2) {
      yy = y1 + (2*(xx-x1)*(y2-y1)+(x2-x1))/(2*(x2-x1));
      data[xx+yy*2*hd->cols] = mag;
      orient[xx+yy*2*hd->cols] = or;
      xx += step;
    }
  }
  else {
    step = 1;
    if (y1 > y2) step = -1;
    yy = y1+step;
    while(yy != y2) {
      xx = x1 + (2*(yy-y1)*(x2-x1)+(y2-y1))/(2*(y2-y1));
      data[xx+yy*2*hd->cols] = mag;
      orient[xx+yy*2*hd->cols] = or;
      yy += step;
    }
  }
}

int CheckALine(int x1, int y1, int x2, int y2,
	       unsigned char *data,
	       unsigned char *orient,
	       struct header *hd)
{
  int i, j, k;
  int step;
  int xx, yy;
  if (x1==x2 && y1==y2) return;
  if (abs(x2-x1)>=abs(y2-y1)) {
    step = 1;
    if (x1>x2) step = -1;
    xx = x1+step;
    while(xx != x2) {
      yy = y1 + (2*(xx-x1)*(y2-y1)+(x2-x1))/(2*(x2-x1));
      if (data[xx+yy*2*hd->cols] !=0) return 1;
      xx += step;
    }
  }
  else {
    step = 1;
    if (y1 > y2) step = -1;
    yy = y1+step;
    while(yy != y2) {
      xx = x1 + (2*(yy-y1)*(x2-x1)+(y2-y1))/(2*(y2-y1));
      if (data[xx+yy*2*hd->cols] !=0) return 1; 
      yy += step;
    }
  }
  return 0;
}