/********************************************************************************/
/********************************* UTILITIES ************************************/

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

typedef float imagetype;
typedef double real;

extern void fifconv(), fifwrite();

char *alloc(), **ialloc(), ***iiialloc();
int inbounds();
void gradient(), ifree(), iiifree(), write_image(), write_imagePGM(),read_image(), basename();
void init_image(), write_block(), read_sal_edgim(), scanls_max_no_last(),read_imagePGM();
int getc4PGM();
real dot_product();
int pxon();

char *
alloc(size)
int size;
{
  char *malloc(), *block = malloc(size);

  if(block == (char *) NULL) {
    printf("alloc: memory allocation error--aborting\n");
    kill(getpid(), SIGABRT);
  }
  return block;
}

char **
ialloc(m, n, size)
int m, n, size;
{
  int i, step;
  char **arr = (char **) alloc(m*sizeof(char *)), **ptr,
       *arr_data = (char *) alloc(m*n*size*sizeof(char)), *data_ptr;

  step = n*size;
  ptr = arr;
  data_ptr = arr_data;
  for(i = 0; i < m; i++) {
    *ptr++ = data_ptr;
    data_ptr += step;
  }
  return arr;
}

/* ifree()
 * frees a 2-D array or image allocated by ialloc()
 */
void
ifree(arr)
char **arr;
{
  free(*arr);
  free(arr);
}

/* iiialloc()
 * allocates a 3-D array or image of character objects of the given size
 */
char ***
iiialloc(m, n, p, size)
int m, n, p, size;
{
  int i, j, step;
  char ***arr = (char ***) alloc(m*sizeof(char **)), ***ptr,
       **arr2 = (char **) alloc(m*n*sizeof(char *)), **ptr2,
       *arr3 = (char *) alloc(m*n*p*size*sizeof(char)), *ptr3;

  step = p*size;
  ptr = arr; ptr2 = arr2; ptr3 = arr3;
  for(i = 0; i < m; i++) {
    *ptr++ = ptr2;
    for(j = 0; j < n; j++) {
      *ptr2++ = ptr3;
      ptr3 += step;
    }
  }
  return arr;
}

/* iiifree()
 * frees a 3-D array allocated by iiialloc()
 */
void
iiifree(arr)
char ***arr;
{
  free(**arr); free(*arr); free(arr);
}

int
inbounds(x,y,w,h)
int x,y,w,h;
{
  return(x >= 0 && x < w && y >= 0 && y < h);
}

real
dot_product(x1,y1,x2,y2)
real x1,y1,x2,y2;
{
  return(x1*x2 + y1*y2);
}

/* From Horn's Robot Vision Sec. 8.3 (p.164)
 * The border pixels will be incorrect.
 */
void
gradient(imdx,imdy, im, w, h)
imagetype **imdx,**imdy, **im;
int w, h;
{
  int i,j;
  real eps = 1.;

  for(i=0; i<w-1; i++) for(j=0; j<h-1; j++) {
      imdx[i][j] = (1/(2*eps))*((im[i+1][j+1] - im[i][j+1]) + (im[i+1][j] - im[i][j]));
      imdy[i][j] = (1/(2*eps))*((im[i+1][j+1] - im[i+1][j]) + (im[i][j+1] - im[i][j]));
    }

  /* set boundary pixels to 0 */
  for(i=0; i<w-1; i++)
    imdx[i][h-1] = imdy[i][h-1] = 0.;
  for(j=0; j<h-1; j++)
    imdx[w-1][j] = imdy[w-1][j] = 0.;
  imdx[w-1][h-1] = imdy[w-1][h-1] = 0.;
}

void
init_image(im, w,h)
imagetype **im;
int w,h;
{
  int i,j;

  for(i=0; i<w; i++) for(j=0; j<h; j++) {
    im[i][j] = 0.;
  }
}

void
write_block(x,y, val, im, w,h)
int x,y, w,h;
real val;
imagetype **im;
{
  if(inbounds(x, y,  w,h)) im[x][y]   = (imagetype) val;
  /*if(inbounds(x, y+1, w,h)) im[x][y+1] = (imagetype) val;*/
  /*if(inbounds(x, y-1, w,h)) im[x][y-1] = (imagetype) val;*/
  /*if(inbounds(x+1, y, w,h)) im[x+1][y] = (imagetype) val;*/
  /*if(inbounds(x-1, y, w,h)) im[x-1][y] = (imagetype) val;*/
}

/* Assumes imagetype = float
 */
void
read_image(image, file, w, h)
int *w, *h;
char *file;
imagetype ***image;
{
  FILE *fp;
  char **env;

  if((fp = fopen(file,"r")) == NULL) {
    fprintf(stderr, "read_image: cannot open file %s\n", file);
    exit(-1);
  }
  fifconv(fp,image,w,h,&env);
  fclose(fp);
}


int getc4PGM(infile)
FILE *infile;
{
  int ch;

  ch = getc(infile);
  if (ch == '#') {
    do {
      ch = getc(infile);
    } while (ch != '\n' && ch != EOF);
  }
  return ch;
}
void
read_imagePGM(image, file, w, h)
int *w, *h;
char *file;
imagetype ***image;
{
  FILE *fp;
  char **env;
  int i,j;
  unsigned char temp;
  int ch;
  int val;
  float **result;
  int w1,h1,maxval;

  if((fp = fopen(file,"r")) == NULL) {
    fprintf(stderr, "read_image: cannot open file %s\n", file);
    exit(-1);
  }
  if (getc(fp) != 'P')
  {
    fprintf(stderr, "read_image: file %s is not in PGM format\n", file);
	exit(-1);
  }
  if (getc(fp) != '5')
  {
    fprintf(stderr, "read_image: file %s is not in PGM raw format\n", file);
	exit(-1);
  }
 do {
    ch = getc4PGM(fp);
    if (ch == EOF)
	{
      fprintf(stderr, "read_image: cannot header of file %s\n", file);
	  exit(-1);
	}
  } while (ch == ' ' || ch == '\t' || ch == '\n' || ch == '\r');

  if (ch < '0' || ch > '9')
  {
    fprintf(stderr, "read_image: cannot header of file %s\n", file);
	exit(-1);
  }

  w1 = ch - '0';
  while ((ch = getc4PGM(fp)) >= '0' && ch <= '9') {
    w1 *= 10;
    w1 += ch - '0';
  }

/***************  READ h *****************************/

do {
    ch = getc4PGM(fp);
    if (ch == EOF)
	{
      fprintf(stderr, "read_image: cannot header of file %s\n", file);
	  exit(-1);
	}
  } while (ch == ' ' || ch == '\t' || ch == '\n' || ch == '\r');

  if (ch < '0' || ch > '9')
  {
    fprintf(stderr, "read_image: cannot header of file %s\n", file);
	exit(-1);
  }

  h1 = ch - '0';
  while ((ch = getc4PGM(fp)) >= '0' && ch <= '9') {
    h1 *= 10;
    h1 += ch - '0';
  }
/*******************************************************************/

do {
    ch = getc4PGM(fp);
    if (ch == EOF)
	{
      fprintf(stderr, "read_image: cannot header of file %s\n", file);
	  exit(-1);
	}
  } while (ch == ' ' || ch == '\t' || ch == '\n' || ch == '\r');

  if (ch < '0' || ch > '9')
  {
    fprintf(stderr, "read_image: cannot header of file %s\n", file);
	exit(-1);
  }

  maxval = ch - '0';
  while ((ch = getc4PGM(fp)) >= '0' && ch <= '9') {
    maxval *= 10;
    maxval += ch - '0';
  }

/*******************************************************************/
  if (maxval!=255)
  {
    fprintf(stderr, "read_image: Use 8bit raw PGM format only\n", file);
	exit(-1);
  }



/*  printf("Width %d length  %d max val %d PGM image \n",w1,h1,maxval);*/
   /*temp = ucifnhread(fp,w1,h1);*/
   result = fialloc(w1,h1);


   for(i=0;i<h1;i++)  
	   for(j=0;j<w1;j++)
	   {
		   if (!feof(fp))
		   fscanf(fp,"%c",&temp);
			result[j][h1-i-1]=(float)temp;
	    } 


/*
   for(i=0;i<w1;i++)
		{
			printf("\n");
		   for(j=0;j<h1;j++)
		   {
			   result[i][h-j-1]=(float)temp[i][j];
			   result[i][j]=
			   printf("%d ",temp[i][j]);
		   }
		}
		ucifree(temp);
		*/
   *image=result;
   *h=h1;
   *w=w1;
 fclose(fp);
}
/* Assumes imagetype = float
 */
void
write_image(image, file, w, h, header)
int w,h;
imagetype **image;
char *file, *header;
{
  FILE *fp;

  if((fp = fopen(file,"w")) == NULL) {
    fprintf(stderr, "write_image: cannot open file %s\n", file);
    exit(-1);
  }
  fprintf(fp, "header=%s", header);
  fifwrite(fp,image,w,h);
  fclose(fp);
}
void
write_imagePGM(image, file, w, h, header)
int w,h;
imagetype **image;
char *file, *header;
{
  FILE *fp;
  int i,j;
  char mfile[500];
  sprintf(mfile, "%s.pgm", file);
  float MaxV=-1;
  float MinV=99999999;

  if((fp = fopen(mfile,"w")) == NULL) {
    fprintf(stderr, "write_image: cannot open file %s\n", file);
    exit(-1);
  }

  for(i=0;i<h;i++)  
	  for(j=0;j<w;j++)
	  {
		  if  (image[j][h-i-1]>MaxV) MaxV=image[j][h-i-1];
		  if  (image[j][h-i-1]<MinV) MinV=image[j][h-i-1];
	  } 
	  


/*  printf("PGM maxval %f minval %f\n",MaxV,MinV);*/
  fprintf(fp, "P5 %d %d %d\n",w,h,255);


   for(i=0;i<h;i++)  
   {
	   /*printf("\n");*/
	   for(j=0;j<w;j++)
		{
			/*printf("%f",image[j][h-i-1]);*/
			fprintf(fp,"%c",(char)((image[j][h-i-1]-MinV)*(255.0/(MaxV-MinV))));
		} 

   }
	  /*fwrite((int)image[i][j],sizeof(int),1,fp);*/
	  
/*	  fwrite(image[i],sizeof (float),h,fp);
  fifwrite(fp,image,w,h);*/
  fclose(fp);
}
void
basename(base,filename)
char **base, *filename;
{
  char *ptr;

  if((ptr = strrchr(filename,'/')) == (char *) NULL)
    *base = filename;
  else
    *base = ptr + 1;
}

int
pxon(x,y,im,w,h)
int x,y,w,h;
imagetype **im;
{
  return(inbounds(x,y,w,h) && im[x][y]);
}

void
read_sal_edgim(file, xs_ad,ys_ad,ks_ad, w_ad, h_ad, num_ad, K)
char    *file;                 /* input file listing the active elements */
int     **xs_ad,**ys_ad,**ks_ad;
int     *w_ad, *h_ad;          /* dimensions of the underlying image */
int     *num_ad;
int     K;                     /* number of orientations */
{
  int x,y, w,h, i, *xs,*ys,*ks, n;
  imagetype **im;

  read_image(&im, file, w_ad, h_ad); w = *w_ad; h = *h_ad;
  n = w*h*K;
  *xs_ad = xs = (int *) alloc(n * sizeof(int));
  *ys_ad = ys = (int *) alloc(n * sizeof(int));
  *ks_ad = ks = (int *) alloc(n * sizeof(int));

  i = 0;
  if(K == 16) {
    for(x=0;x<w;x++) for(y=0;y<h;y++) if(pxon(x,y,im,w,h)) {
      if(pxon(x+1,y,im,w,h) && pxon(x+2,y,im,w,h))      {xs[i]=x;ys[i]=y;ks[i]= 0;i++;}
      if(pxon(x+2,y+1,im,w,h))
	 {xs[i]=x;ys[i]=y;ks[i]= 1;i++;}
      if(pxon(x+1,y+1,im,w,h) && pxon(x+2,y+2,im,w,h))  {xs[i]=x;ys[i]=y;ks[i]= 2;i++;}
      if(pxon(x+1,y+2,im,w,h))
	 {xs[i]=x;ys[i]=y;ks[i]= 3;i++;}

      if(pxon(x,y+1,im,w,h) && pxon(x,y+2,im,w,h))      {xs[i]=x;ys[i]=y;ks[i]= 4;i++;}
      if(pxon(x-1,y+2,im,w,h))
	 {xs[i]=x;ys[i]=y;ks[i]= 5;i++;}
      if(pxon(x-1,y-1,im,w,h) && pxon(x-2,y-2,im,w,h)) {xs[i]=x;ys[i]=y;ks[i]= 6;i++;}
      if(pxon(x-2,y+1,im,w,h))
	 {xs[i]=x;ys[i]=y;ks[i]= 7;i++;}

      if(pxon(x-1,y,im,w,h) && pxon(x-2,y,im,w,h))      {xs[i]=x;ys[i]=y;ks[i]= 8;i++;}
      if(pxon(x-2,y-1,im,w,h))
	 {xs[i]=x;ys[i]=y;ks[i]= 9;i++;}
      if(pxon(x-1,y-1,im,w,h) && pxon(x-2,y-2,im,w,h))  {xs[i]=x;ys[i]=y;ks[i]=10;i++;}
      if(pxon(x-1,y-2,im,w,h))
	 {xs[i]=x;ys[i]=y;ks[i]=11;i++;}

      if(pxon(x,y-1,im,w,h) && pxon(x,y-2,im,w,h))     {xs[i]=x;ys[i]=y;ks[i]=12;i++;}
      if(pxon(x+1,y-2,im,w,h))
	 {xs[i]=x;ys[i]=y;ks[i]=13;i++;}
      if(pxon(x+1,y-1,im,w,h) && pxon(x+2,y-2,im,w,h)) {xs[i]=x;ys[i]=y;ks[i]=14;i++;}
      if(pxon(x+2,y-1,im,w,h))
	 {xs[i]=x;ys[i]=y;ks[i]=15;i++;}
    }}
  else {
    fprintf(stderr,"read_sal_edgim: Cannot handle %d orientations\n", K);
    exit(1); }
  *num_ad = i;
}

/* will not write to im[x2][y2]
 */
void
scanls_max_no_last(x1,y1,x2,y2,im,n,val)
int x1,y1,x2,y2, n;
imagetype **im, val;
{
#define round(x) ((x) >= 0 ? (int) ((x) + .5) : (int) ((x) - .5))

  int i;
  double x,y, th, Cth, Sth, dist, step;

  dist = sqrt((double) (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
  step = dist/(n-1);
  th = atan2((double) y2-y1, (double) x2-x1);
  Cth = cos(th); Sth = sin(th);

  for(i=0, x=x1, y=y1; i < n; i++, x += step*Cth, y += step*Sth)
    if(round(x) == x2 && round(y) == y2)
      break;
    else if(val > im[round(x)][round(y)])
      im[round(x)][round(y)] = val;
}
