
/* At MIT compiled with:
 *      cc -c sumap_read.c utilities.c
 *      cc susal.c utilities.o sumap_read.o -o susal -lvis -lvio -lm
 *
 * At NEC, from iris65 or from iris61, compiled with:
 *      cc -c sumap_read.c utilities.c
 *      cc susal.c utilities.o sumap_read.o -o susal /vision/src/tmb_vis/lib/libvis.a -lm
 */

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

typedef float imagetype;
typedef double real;

extern int inbounds();
extern char *alloc(), **ialloc(), ***iiialloc();
extern void figauss();
extern void gradient(), ifree(), iiifree(), write_image(), write_imagePGM(),read_image(), basename();
extern void init_image(), write_block(), read_sal_input_file(), read_sal_edgim();
extern void read_sal_input_file_wh();
extern real dot_product();


struct selement {
  real sigma, rho, sal;
  int neigh;
};

typedef struct selement element;


#define mod(x,b) ((x) >= 0 ? (x) % (b) : (((x)%(b) + (b))%(b)))

#define directional_deriv(dx,dy,th) (dot_product(dx,dy, cos(th),sin(th)))

#define BORDER 10            /* arbitrary */






/* Freeman: (note that to make all the line elements roughly the same length,
 *   we skip a pixel at some points where we don't have to).
 *
 *    Y|X=  -2  -1   0   1   2
 *    -|----------------------
 *    2|     6   5   4   3   2
 *     |
 *    1|     7   .   .   .   1
 *     |
 *    0|     8   .   X   .   0
 *     |
 *   -1|     9   .   .   .  15
 *     |
 *   -2|    10  11  12  13  14
 */
static int
  Xoffset16[] = {2, 2, 2, 1, 0, -1, -2, -2, -2, -2, -2, -1,  0,  1,  2,  2},
  Yoffset16[] = {0, 1, 2, 2, 2,  2,  2,  1,  0, -1, -2, -2, -2, -2, -2, -1};

/* Skipping 3 pixels
 *
 *    Y|X=  -3  -2  -1   0   1   2   3
 *    -|------------------------------
 *    3|     9   8   7   6   5   4   3
 *     |
 *    2|    10   .   .   .   .   .   2
 *     |
 *    1|    11   .   .   .   .   .   1
 *     |
 *    0|    12   .   .   X   .   .   0
 *     |
 *   -1|    13   .   .   .   .   .  23
 *     |
 *   -2|    14   .   .   .   .   .  22
 *     |
 *   -3|    15  16  17  18  19  20  21
 */
static int
  Xoffset24[] = {3, 3, 3, 3, 2, 1, 0, -1, -2, -3, -3, -3, -3, -3, -3, -3, -2, -1,  0,  1,  2,  3,  3,  3},
  Yoffset24[] = {0, 1, 2, 3, 3, 3, 3,  3,  3,  3,  2,  1,  0, -1, -2, -3, -3, -3, -3, -3, -3, -3, -2, -1};

static int *Xoffset, *Yoffset;
static real **F_coef, Pi;

element ***make_map();
void set_f_coefs(), free_map(), init_map(), compute_saliency_map(), map_to_image();


/* K = # orientations. Assumes K is even.
 */
void
set_f_coefs(K, delta_elt)
int K, delta_elt;
{
  int i,j;
  real delta_angle, alpha;
  real *angle = (real *) alloc(K*sizeof(real)); /* change in angle from element 0 to i */

  delta_angle = 2*Pi/K;
  angle[0] = 0.; angle[K/2] = Pi; 
  for(i=1; i < K/2; i++) {
    angle[i] = i*delta_angle; angle[K - i] = -i*delta_angle; }

  F_coef = (real **) ialloc(K,K,sizeof(real));
  for(i=0;i<K;i++) for(j=0;j<K;j++) {
    alpha = fabs(angle[mod(j - i, K)]);
    F_coef[i][j] = exp(-2*alpha*tan(alpha/2)/delta_elt); }
  free(angle);
}    


/* K = # orientations
 */
element ***
make_map(w,h,K)
int w,h,K;
{
  return (element ***) iiialloc(w,h,K,sizeof(element));
}


void
free_map(map)
element ***map;
{
  iiifree((char ***)map);
}


/* K = # orientations
 */
void
init_map(map, image, w,h,K, sigma, rho)
element ***map;
imagetype **image;
int w,h, K, sigma;
real rho;
{
  int i,j,k, active_element();
  imagetype **imdx, **imdy, thresh, magsum;
  element *elt_a;
  real delta_angle, th;

  imdx = (imagetype **) ialloc(w,h,sizeof(imagetype));
  imdy = (imagetype **) ialloc(w,h,sizeof(imagetype));
  gradient(imdx,imdy, image, w,h);

  magsum = (imagetype) 0;
  for(i=0;i<w;i++) for(j=0;j<h;j++)
    magsum += sqrt(imdx[i][j]*imdx[i][j] + imdy[i][j]*imdy[i][j]);
  thresh = magsum/(w*h); /* average gradient magnitude */
  printf(" init_map: gradient magnitude thresh= %f\n", thresh);

  delta_angle = 2*Pi/K;
  for(i=0;i<w;i++) for(j=0;j<h;j++) for(k=0; k<K; k++) { /* run through elements */
    th = k*delta_angle;
    elt_a = &(map[i][j][k]);
    if(active_element(imdx[i][j],imdy[i][j], th, thresh)) {
      elt_a->rho = 1;   elt_a->sigma = elt_a->sal = (real) sigma; }
    else {
      elt_a->rho = rho; elt_a->sigma = elt_a->sal = 0; }
    elt_a->neigh = -1; }

  ifree((char **)imdx); ifree((char **)imdy);
}


/* K = # orientations
 * inpflag = 2 ==> use tao's method, = 3 (or anything else) ==> use Freeman's method
 */
void
init_map_intensity(map, image, w,h,K, sigma, rho, inpflag)
element ***map;
imagetype **image;
int w,h, K, sigma, inpflag;
real rho;
{
  int i,j,k;
  real delta_angle, th, dderiv;
  imagetype **imdx, **imdy, mag, magmax;
  element *elt_a;

  imdx = (imagetype **) ialloc(w,h,sizeof(imagetype));
  imdy = (imagetype **) ialloc(w,h,sizeof(imagetype));
  gradient(imdx,imdy, image, w,h);

  magmax = (imagetype) 0;
  for(i=0;i<w;i++) for(j=0;j<h;j++) /* compute gradient magnitude maximum value */
    if((mag = sqrt(imdx[i][j]*imdx[i][j] + imdy[i][j]*imdy[i][j])) > magmax)
      magmax = mag;
  if(magmax == 0) {
    fprintf(stderr,"init_map_intensity: All image gradients are 0 vectors. Killing...\n");
    exit(-1); }
  printf(" init_map_intensity: gradient magnitude max= %f\n", magmax);

  delta_angle = 2*Pi/K;
  for(i=0;i<w;i++) for(j=0;j<h;j++) for(k=0; k<K; k++) { /* run through elements */
    th = k*delta_angle;
    elt_a = &(map[i][j][k]);
    dderiv = directional_deriv(-imdy[i][j],imdx[i][j],th)/magmax;
    elt_a->sigma = elt_a->sal = sigma*dderiv;
    elt_a->rho = inpflag == 2 ? pow(rho, 1. - dderiv) : 1.0;
    elt_a->neigh = -1; }

  ifree((char **)imdx); ifree((char **)imdy);
}


int
active_element(dx,dy, th, thresh)
imagetype dx,dy, thresh;
real th;
{
  return directional_deriv(-dy,dx,th) > thresh ? 1 : 0;
}


void
image_to_map(map, im, w,h,K, gsigma, sigma, rho, inpflag)
element ****map;
imagetype **im;
int w,h,K, sigma, inpflag;
real gsigma, rho;
{
  printf("Smoothing image...\n");
  if(gsigma>0.0) figauss(im, w, h, gsigma, gsigma);
  printf("Making map...\n");
  *map = make_map(w,h,K);
  printf("Initializing map...\n");
  if(inpflag == 1)
    init_map(*map, im, w,h,K, sigma, rho);
  else
    init_map_intensity(*map, im, w,h,K, sigma, rho, inpflag);
}


/* Note: input vector sals[i] is multiplied by sigma
 */
void
read_map(map,w,h, file, K, sigma, rho, inpflag)
element ****map;
int *w,*h, K, sigma, inpflag;
char *file;
real rho;
{
  int *xs, *ys, *ks, *neighs, n, i,j,k, xmax,ymax;
  double *sals;
  element *elt_a;

  printf("Reading map data...\n");
  if(inpflag == 0)
    read_sal_input_file(file, &xs, &ys, &ks, &sals, &neighs, &n);
  else if (inpflag == 4) {
    read_sal_edgim(file, &xs,&ys,&ks, w, h, &n, K);
    sals = (double *) NULL; neighs = (int *) NULL; }
  else /* inpflag == 5 */
    read_sal_input_file_wh(file, &xs, &ys, &ks, &sals, &neighs, &n, w, h);

  xmax = xs[0]; ymax = ys[0];
  for(i=1; i < n; i++) {
    if(xs[i] > xmax) xmax = xs[i];
    if(ys[i] > ymax) ymax = ys[i];
  }    
  if(inpflag == 0) {
    *w = xmax + BORDER; *h = ymax + BORDER; }
  printf("Making map...\n");
  *map = make_map(*w,*h,K);

  printf("Initializing map...\n");
  for(i=0;i<*w;i++) for(j=0;j<*h;j++) for(k=0; k<K; k++) { /* run through elements */
    elt_a = &((*map)[i][j][k]);
    elt_a->sigma = elt_a->sal = 0.;                   /* set all elements to be gaps */
    elt_a->rho = rho;
    elt_a->neigh = -1;
  }  

  for(i=0; i<n; i++) {
    elt_a = &((*map)[xs[i]][ys[i]][ks[i]]);
    elt_a->sigma = sigma; elt_a->sal = sigma*sals[i]; /* overwrite with active elements */
    elt_a->rho = 1.;
    if(neighs != (int *) NULL) elt_a->neigh = neighs[i]; }

  free(xs); free(ys); free(ks);
  if(sals != (double *) NULL) free(sals); if(neighs != (int *) NULL) free(neighs);
}

void
compute_saliency_map(map, N, w,h,K, printmod, printmsals, leavebug)
element ***map;
int N,w,h,K, printmod, printmsals, leavebug;
{
  int iter,x,y,k, neigh, max_neigh, xnext,ynext, removebug = leavebug ? 0 : 1;
  element *elt_a, *elt_b;
  real max_val, val, ***tmpsal = (real ***) iiialloc(w,h,K,sizeof(real));
  void most_salient_element();

  for(x=0;x<w;x++) for(y=0;y<h;y++) for(k=0;k<K;k++) /* run through elements */
    tmpsal[x][y][k] = -1.;

  for(iter=1;iter<=N;iter++) {
    for(x=0;x<w;x++) for(y=0;y<h;y++) for(k=0;k<K;k++) { /* run through elements */
      elt_a = &(map[x][y][k]);                /* kth element leaving pixel (x,y) */
      xnext = x + Xoffset[k]; ynext = y + Yoffset[k];
      if(inbounds(xnext, ynext, w,h)) {
	max_neigh = 0; max_val = 0.;
	for(neigh = 0; neigh < K; neigh++) {
	  elt_b = &(map[xnext][ynext][neigh]);
	  val = F_coef[k][neigh]*elt_b->sal;
	  if(removebug)
	    val *= elt_b->rho;
	  if(val > max_val) {
	    max_val = val; max_neigh = neigh; }
	}
      }
      if(inbounds(x + Xoffset[max_neigh], y + Yoffset[max_neigh], w,h)) {
	tmpsal[x][y][k] = elt_a->sigma + (removebug ? max_val : elt_a->rho*max_val);
	elt_a->neigh = max_neigh; }
    }
    for(x=0;x<w;x++) for(y=0;y<h;y++) for(k=0;k<K;k++) /* run through elements */
      map[x][y][k].sal = tmpsal[x][y][k];
    if(iter == 1 || iter%printmod == 0)
      if(printmsals) {
	printf(" Iteration %d. ", iter);
	most_salient_element(&x,&y,&k, map, w,h,K); }
      else
	printf(" Iteration %d\n", iter);
  }
  iiifree((char ***)tmpsal);
}


void
map_to_image(image, map, w,h,K)
imagetype **image;
element ***map;
int w,h,K;
{
  int x,y,k, numpts = K == 16 ? 5 : 7;
  void scanls_max_no_last();

  for(x=0;x<w;x++) for(y=0;y<h;y++) {
    image[x][y] = (imagetype) 0.;
    for(k=0;k<K;k++)
      if(inbounds(x + Xoffset[k], y + Yoffset[k], w,h))
	  {
	scanls_max_no_last(x,y, x+Xoffset[k], y+Yoffset[k], image, numpts, (imagetype) 1);
	  if(map[x][y][k].sal > image[x][y]) image[x][y] = map[x][y][k].sal;
	  }
  }
}


void
most_salient_element(mx,my,mk, map, w,h,K)
int *mx,*my,*mk, w,h,K;
element ***map;
{
  int x,y,k;
  real sal;

  sal = -1.;
  for(x=0;x<w;x++) for(y=0;y<h;y++) for(k=0;k<K;k++) /* run through elements */
    if(map[x][y][k].sal > sal) {
      sal = map[x][y][k].sal; *mx = x; *my = y; *mk = k; }
  printf(" Max saliency=%f  Element=(%d,%d,%d)\n", sal, *mx,*my,*mk);
}


/* Given the starting element and the number of elements to trace,
 * writes the saliencies of the elements onto the image pixels at the tails
 * of the arrows.
 */
void
trace_curve(image, x,y,k, n_steps, map, w,h,K, printcurve)
imagetype **image;
element ***map;
int x,y,k, w,h, n_steps, printcurve;
{
  int i, numpts = K == 16 ? 5 : 7;
  element *elt_a;
  void scanls_max_no_last();

  for(i=0; i < n_steps; i++) {
    elt_a =  &(map[x][y][k]);
    if(printcurve)
      printf("  Elt (%d,%d,%d): sal=%f rho=%.2f sigma=%.3f neigh=%d\n",
	     x,y,k, elt_a->sal, elt_a->rho, elt_a->sigma, elt_a->neigh);
    if(inbounds(x + Xoffset[k], y + Yoffset[k], w,h))
      scanls_max_no_last(x,y, x+Xoffset[k], y+Yoffset[k], image, numpts, (imagetype) 1);
    /* write_block(x,y, 1.0, image, w,h); */
    x += Xoffset[k];
    y += Yoffset[k];
    if(!inbounds(x, y, w,h)) {
      printf("  Curve ran outside image at step %d\n", i);
      return; }
    if(elt_a->neigh == -1) {
      fprintf(stderr, "trace_curve: Current element has no neighbor. ");
      fprintf(stderr, "Step= %d\n", i); }
    k = elt_a->neigh; }
  /* write_block(x,y, 1.0, image, w,h); */
}
  

void
most_salient_curve(image, map, w,h,K, tlen, printcurve)
imagetype **image;
element ***map;
int w,h,K, tlen, printcurve;
{
  int mx,my,mk;

  most_salient_element(&mx,&my,&mk, map, w,h,K);
  trace_curve(image, mx,my,mk, tlen, map, w,h,K, printcurve);
}


void
write_map(file, map, w,h,K, scale)
char *file;
element ***map;
int w,h,K, scale;
{
  int x,y,k;
  char mfile[500];
  FILE *fp;

  sprintf(mfile, "%s.map", file);
  if((fp = fopen(mfile, "w")) == NULL) {
    fprintf(stderr, "write_map: cannot open file %s\n", mfile);
    exit(-1);
  }
  fprintf(fp, "points: %d; dim: 5; w: %d; h: %d; K: %d; scale: %d\n", w*h*K, w,h,K,scale);
  for(x=0;x<w;x++) for(y=0;y<h;y++) for(k=0;k<K;k++) /* run through elements */
    fprintf(fp, "%3d %3d %2d %f %2d %f;\n", x,y,k,
	    map[x][y][k].sal, map[x][y][k].neigh, map[x][y][k].sigma);
  fclose(fp);
}


void
print_salmap(file, map, w, h, K)
char *file;
element ***map;
int w,h,K;
{
#define INBOUNDS( x, y, w, h ) ( (x) >= 0 && (x) < (w) && (y) >= 0 && (y) < (h) )

  int x,y,k, numpts = K == 16 ? 5 : 7;
  imagetype **im;
  element *elt_a;
  char gfile[500];
  void scanls_max_no_last(), init_image(), write_image();

  im = (imagetype **) ialloc(w,h,sizeof(imagetype));
  init_image(im, w,h);

  elt_a = **map;
  for(x=0;x<w;x++) for(y=0;y<h;y++) for(k=0;k<K;k++) { /* run through elements */
    if(INBOUNDS(x+Xoffset[k],y+Yoffset[k],w,h))
      scanls_max_no_last(x,y, x+Xoffset[k], y+Yoffset[k], im, numpts,
			 (imagetype) elt_a->sigma);
    elt_a++; }

  sprintf(gfile, "%s_gph", file);
  write_image(im, gfile, w, h, "\n");
  printf("Writing map:\n");

  ifree((char **)im);
  printf(" Graph written to %s\n", gfile);
}


void
read_map_params(file, inpflag,gsigma,N,K,tlen,printmod,printmsals,printcurve,leavebug,
		scale,printmap)
char *file;
double *gsigma;
int *inpflag,*N,*K,*tlen,*printmod,*printmsals,*printcurve,*leavebug,*scale,*printmap;
{
  FILE *fp;
  char line[100];
  int line_size = 100;

  if((fp = fopen(file ,"r")) == NULL) {
    fprintf(stderr, "read_map_params: cannot open file %s\n", file);
    exit(-1);
  }
  fgets(line, line_size, fp); sscanf(line, "%d", inpflag);
  fgets(line, line_size, fp); sscanf(line, "%lf", gsigma);
  fgets(line, line_size, fp); sscanf(line, "%d", N);
  fgets(line, line_size, fp); sscanf(line, "%d", K);
  fgets(line, line_size, fp); sscanf(line, "%d", tlen);
  fgets(line, line_size, fp); sscanf(line, "%d", printmod);
  fgets(line, line_size, fp); sscanf(line, "%d", printmsals);
  fgets(line, line_size, fp); sscanf(line, "%d", printcurve);
  fgets(line, line_size, fp); sscanf(line, "%d", leavebug);
  fgets(line, line_size, fp); sscanf(line, "%d", scale);
  fgets(line, line_size, fp); sscanf(line, "%d", printmap);
  fclose(fp);
  printf("inpflag=%d gsigma=%.3f N=%d K=%d tlen=%d printmod=%d printmsals=%d",
	 *inpflag,*gsigma,*N,*K,*tlen,*printmod,*printmsals);
  printf(" printcurve=%d leavebug=%d scale=%d printmap=%d\n",
	 *printcurve,*leavebug,*scale,*printmap);
}


void
print_program_info()
{
  fprintf(stderr," typical args: straight-10 straight-10.su su-params\n\n");
  fprintf(stderr," Computes Shashua and Ullman's saliency map\n");
  fprintf(stderr," Writes a saliency image into <outfile> (an 8 bit raw PGM image)\n");
  fprintf(stderr," Writes the most salient curve into <outfile>_cur ");
  fprintf(stderr,"(an 8 bit raw PGM image)\n");
  fprintf(stderr," Typical parameter file (expects each parameter to start a line):\n");
  fprintf(stderr,"  0      0/1: Input is a saliency map 0 or an 8 bit raw PGM image \n");
  fprintf(stderr,"  1      sigma for guassian smoothing (ignored if input is sal map)\n");
  fprintf(stderr,"  50     number of iterations (N)\n");
  fprintf(stderr,"  16     16/17/24/25: Number of discrete orientations is 16 (16/17) ");
  fprintf(stderr,"or is 24 (24/25)\n");
  fprintf(stderr,"          Also, iff odd (17/24) then reset gap penalty, rho, to 0\n");
  fprintf(stderr,"  50     length to trace (typically = the number of iterations)\n");
  fprintf(stderr,"  10     printmod. Prints iteration number after every printmod ");
  fprintf(stderr,"iterations\n");
  fprintf(stderr,"  1      0/1: Iff 1, then compute and print max saliency ");
  fprintf(stderr,"every printmod iterations\n");
  fprintf(stderr,"  1      0/1: Iff 1, when done print to stdout the elements on ");
  fprintf(stderr,"the most salient curve\n");
  fprintf(stderr,"  0      0/1: Iff 1, leave the bug from Shashua & Ullman's paper\n");
  fprintf(stderr,"  1	   scale (multiplies element length)\n");
  fprintf(stderr,"  1      0/1: Print the saliency map to <outfile>.map\n");
  putchar('\n');
}


main(argc, argv)
int argc;
char **argv;
{
  char *base, *infile, *outfile, *parfile, header[500], curvefile[500];
  int w,h, inpflag,N,K,tlen,printmod,printmsals,printcurve,delta_elt,sigma,leavebug;
  int i, scale, printmap;
  element ***map;
  imagetype **im;
  real gsigma, rho;

  if(argc == 1) { /* no args given */
    basename(&base,argv[0]);
    fprintf(stderr,"usage: %s infile outfile parameter-file\n", base);
    print_program_info();
    exit(0);
  }
  infile = argv[1];
  outfile = argv[2];
  parfile = argv[3];

  Pi = acos(-1.);
  read_map_params(parfile,&inpflag,&gsigma,&N,&K,&tlen,&printmod,&printmsals,&printcurve,
		  &leavebug,&scale,&printmap);
  switch(K) {
    case 16: Xoffset = Xoffset16; Yoffset = Yoffset16;
             delta_elt = 2; sigma = 2; rho= .49; break;
    case 17: Xoffset = Xoffset16; Yoffset = Yoffset16;
             delta_elt = 2; sigma = 2; rho= 0.; break;
    case 24: Xoffset = Xoffset24; Yoffset = Yoffset24;
             delta_elt = 3; sigma = 3; rho= .343; break;
    case 25: Xoffset = Xoffset24; Yoffset = Yoffset24;
             delta_elt = 3; sigma = 3; rho= 0.; break;
    default: fprintf(stderr, "Cannot handle %d orientations. exiting...\n", K); exit(1); }
  for(i=0;i<K;i++) {
    Xoffset[i] *= scale; Yoffset[i] *= scale; }
  delta_elt *= scale; sigma *= scale; rho = (real) pow((double) rho, (double) scale);
  set_f_coefs(K,delta_elt);

  if(inpflag == 0 || inpflag == 4 || inpflag == 5) {
    read_map(&map, &w, &h, infile, K, sigma, rho, inpflag);
    im = (imagetype **) ialloc(w,h,sizeof(imagetype)); }
  else {
    /*read_image(&im, infile, &w, &h);*/
  read_imagePGM(&im, infile, &w, &h);
  image_to_map(&map, im, w,h,K, gsigma, sigma, rho, inpflag); }
  printf("Computing map...\n");
  compute_saliency_map(map, N, w,h,K, printmod, printmsals, leavebug);
  printf("Converting to output image...\n");
  map_to_image(im, map, w,h,K);
 /* print_salmap(outfile, map, w, h, K);*/
  printf("Writing image...\n");
 /* sprintf(header, "(* infile=%s outfile=%s inpflag=%d gsigma=%f N=%d K=%d tlen=%d printmod=%d printmsals=%d printcurve=%d leavebug=%d sigma=%d rho=%f *)\n", infile, outfile, inpflag, gsigma, N, K, tlen, printmod, printmsals, printcurve, leavebug, sigma, rho);
  write_image(im, outfile, w, h, header);*/
  write_imagePGM(im, outfile, w, h, header);
  printf("Initializing image...\n");
  init_image(im, w,h);
  printf("Finding the most salient curve...\n");
  most_salient_curve(im, map, w,h,K, tlen, printcurve);

  printf("Writing image...\n");
  sprintf(curvefile, "%s_cur", outfile);
  /*write_image(im, curvefile, w, h, header);*/
  write_imagePGM(im, curvefile, w, h, header);
  if(printmap)
    write_map(outfile, map, w,h,K, scale);

  ifree((char **)F_coef); free_map(map); ifree((char **)im);
}
