/* 
Lightness computation - Andrew Blake, Dept. CS, Edinburgh University.
adjusted by Gavin Brelstaff 16/11/84.

Altered to allow for a SQUARE GRID OF SKEWED TRIANGLES rather than
equilaterals.

  DISCARDED 16/9/85
  WITH NON-FLOATING POINT VARIATION OF THE OVERRELAXTION PARAMETER,
  implemented by Gavin Brelstaff 21/11/84.

Extended to pictures greater than 128x128 by Gavin Brelstaff 14/12/84.

WITH LOGARITHMIC INPUT CONVERSION
WITH OUTPUT OF MEAN ZERO            both by Gavin Brelstaff   14/1/85
 
With threshold percentage option 4/2/85 Gavin Brelstaff.

DEVELOPMENT CANNY THRESHOLD FILE INPUT OPTION 27/2/85 Gavin Brelstaff
MORE DEVELOPMENT OF CANNY 19/3/85

picture format:
  first byte is image size (square image) if less than 128, else use -x option,
  then image grey data in left to right, top to bottom format.

to run, 

  see vision:light.doc

*/
#include <stdio.h>
#include <math.h>  /* for log function */
#define progname "light"

#define flagU 0100
#define flagD 0200
#define MAXSIZE 256 /* max. image size */
#define MAXAREA MAXSIZE*MAXSIZE
#define MAXCHAR 255
#define EOS '\0'

short nbits = 0;                /* picture scaling: nbits +8 bits */
long x[MAXAREA], rij[MAXAREA] ;  /* pics: relaxing, initial */
char conflag[MAXAREA];          /* flags indicating existence of triangles
                                   and results of thresholding (top 2 bits)
                                   uij & dij */
char Mij[MAXAREA];             /* number of surrounding triangles*/

short asize ;    /* pic dimensions */
long area;
short header=1;/* set if pic file contains a header char */
     
float th_percentage=6.0;         /* input parameter the threshold percentage*/
int th=100;                      /* threshold value for (gradient*gradient) */
#define MAXCHANGE 0
#define MAXITER 10000
int iter=0;                     /* maximum no of iterations */
int minchange=MAXCHANGE;        /* min max-change for one iteration: stops */
                                /* program if max-change is less */
short freq=10;                  /* number of iters per reported change */
int prec=16;                    /* constant specifying precision of integer
                                   computation */

FILE *fp, *fq = NULL;            /* input,output picture channels */
 

char canny_file_name[32];  /* name file containing canny results */
#define CANNYFILE "canny.hyst"   /* default canny file name */
char funcflag=0;/* set if Functional is to be computed */
char cflag=0;   /* set if canny operator file is used as thresholding */
char gflag=0;   /* set if canny operator file is used to guide thresholding */
char tflag=0;   /* set if threshold specified */
char mflag=1;   /* set if mean zero option for output */
char bflag=0;   /* set if beeps for attention */
char sflag=0;   /* set if output to screen is to be supressed */
char lflag=1;   /* set if log conversion used */
char wflag=0;   /* set if writing threshold flags to file */
char fflag=0;   /* set if reading threshold flags from file */
char edge_file_name[32];  /* file name to send edge results to if wflag=1 */
char flag_file_name[32];  /* file name to get edges from if fflag=1 */
#define EDGEFILE "edgeU"   /* default edge file name */
#define FLAGFILE "flagU"   /* default flag file name */

 
 
main(argc,argv)
char **argv;
{

  argv++;
  while ((argc>1)&&((*argv)[0] == '-'))
  {

    switch((*argv)[1]) {
    case 'j':
      error("SOR always 0.5");
      break;
    case 'x':
      sscanf(*argv+2,"%hd",&asize);
      if(asize<0||asize>MAXSIZE) error("picture to big");
      header=0;
      break;
    case 'F':
      sscanf(*argv+2,"%hd",&freq);
      if(freq<1) error("bad reporting frequency");
      break;
    case 'c':
      sscanf(*argv+2,"%s",canny_file_name);
      cflag=1;
      break;
    case 'g':
      sscanf(*argv+2,"%s",canny_file_name);
      gflag=1;
      break;
    case 's':  /* supress output to screen flag */
      sflag=1;
      break;
    case 'w':  /* write threshold flag files */
      sscanf(*argv+2,"%s",edge_file_name);
      wflag=1;
      break;
    case 'f':  /* read threshold flag files */
      sscanf(*argv+2,"%s",flag_file_name);
      fflag=1;
      break;
    case 'l':  /* turn off logarithmic input conversion option */
      lflag=0;
      break;
    case 'm':  /* turn off mean-zero output option */
      mflag=0;
      break;
    case 't':
      sscanf(*argv+2,"%f",&th_percentage);
      if(th_percentage<0.0)
        error("bad threshold percentage");
      tflag=1;
      break;
    case 'i':
      sscanf(*argv+2,"%d",&iter);
      if(iter<0||MAXITER<iter) error("bad number of iterations");
      break;
    case 'M':
      sscanf(*argv+2,"%d",&minchange);
      if(minchange<0) error("bad minchange");
      break;
    case 'p':
      sscanf(*argv+2,"%d",&prec);
      if(prec<0||16<prec) error("bad precision specified");
      break;
    case 'G': /* switch on Functional report */
      funcflag=1;
      break;
    default:
      error("unknown flag");
      break;
    }
    --argc;argv++;
  }
 
  if (argc<3)
    error("missing arguments");

  if((fp = fopen(*argv++,"r")) == NULL)
    error("can't access input file");
  --argc;

  if((fq = fopen(*argv,"w")) == NULL)
    error("can't access output file");
  fclose(fq);

  if((cflag|gflag)&&(canny_file_name[0] == EOS)) {
    strcpy(canny_file_name,CANNYFILE);
    if(!sflag) fprintf(stderr,"using %s as default\n",CANNYFILE);
  }
  if(wflag&&(edge_file_name[0] == EOS)) {
    strcpy(edge_file_name,EDGEFILE);
    if(!sflag) fprintf(stderr,"using %s as default\n",EDGEFILE);
  }
  if(fflag&&(flag_file_name[0] == EOS)) {
    strcpy(flag_file_name,FLAGFILE);
    if(!sflag) fprintf(stderr,"using %s as default\n",FLAGFILE);
  }

  readpic();
  fclose(fp);
  log_scale(lflag,tflag);

  connect();
  threshold();
  if(funcflag) functional();
  if(wflag) write_flags();
  make_Rij();
  scale(prec);
  iterate();
  scale(0);
  if(!mflag)  makepos(x);
  else make_meanzero(x);

  if((fq = fopen(*argv++,"w")) == NULL)
    error("can't reaccess output file");
  writepic(x);
  fclose(fq);
}
 

makepos(y)  /* ensure pic is everywhere positive and in range */
long *y;
{
long *p, max, min, *pend;

  min=100000; max = -100000;
  for (p=y,pend=y+area; p<pend; p++)
  {
    if (*p > max) max = (*p);
    if (*p < min) min = (*p);
  }

  for (p=y,pend=y+area; p<pend; p++)
    *p = (*p) -min;

  max=max-min;
  while (max>MAXCHAR) {
    scale(nbits-1);
    max = ((max+1)>>1);
  }
}

#define TMAX 256
#define TOP_INPUT_INTENSITY 256

log_scale(lflag,tflag)
char lflag,tflag;
{
long log_table[TMAX];  /* temporary log conversion look up table */
double CONST;
register long  *p;
register long i;

    if(!lflag){
      nbits=0;
      if(tflag)
         th = ((int) th_percentage);  /* set threshold option */
      return(0);  
    }

    /* 2^11 constant ensures discrimination between log values */
    /* obtained from adjacent input levels: because without    */
    /* any mult-factor range would be 0 .. 8                   */

    nbits = 6;     /* 11 + 3 - 8 */    /* global variable */
    if(tflag)  th=((int) ( (TOP_INPUT_INTENSITY<<nbits)*
                            log( (double) (1.0 + th_percentage/100.0))/
                            log( (double) TOP_INPUT_INTENSITY) ) );
    th=th*th;

    log_table[0]=0;
    CONST = 2048.0/log((double) 2.0) ;  /* 2^11/loge2 */
    for(i=1,p=log_table+1;i<TMAX;i++) 
      *p++ = ( (long) (log((double) i)*CONST));

    for (p=x,i=0;i<area;i++ )
      *p++ = log_table[*p];

/*
    for(i=0,p=log_table;i<TMAX;i++)
       if(!sflag) fprintf(stderr,"%d %ld\n",i,*p++);
*/   
}

error(s)
char *s;
{
  fprintf(stderr,"%s: %s\n",progname,s);
  exit(1);
}
 
readpic()       /*read from fp, set up x as integer arrays*/
{
 
register long *p;
long i, *pend;
 
 
  if (header)    asize=getc(fp)&0377;
  if(asize<0||asize>MAXSIZE) error("picture to big");
  area = asize*asize;
 
  for (p=x,pend=x+area; p<pend;)
    *p++ = 0377& getc(fp);
}
 
writepic(y)      /* send y out on fq. low bytes only */
long *y;
{
 
long *p;
long *pend;
 
  if(header||(asize<MAXCHAR))putc(asize,fq);
 
  for (p=y, pend=y+area; p<pend;)
    putc(*p++,fq);
}


make_meanzero(y)  
long *y;
{
long *p , *pend,total,mean;

  for (p=y,pend=y+area,total=0; p<pend; p++)
    total += *p;

  mean= total/area;

  for (p=y,pend=y+area; p<pend; p++) {
    *p = (*p) -mean;
    if(*p>127) *p =  127;
    else if(*p<-128) *p = -128;
  }
}
 

scale(n)  /* scale x,rij to n+8 bits */
{
register long s, *ptop;
register long *p,*q;
 
  s=n-nbits;
  nbits=n;
 
  if (s>0)
  {
    for (p=x,q=rij,ptop=x+area; p<ptop;)
    {
      *p++ = (*p<<s);
      *q++ = (*q<<s);
    }
  }
  else if (s<0)
  {
    s= -s;
    for (p=x,q=rij,ptop=x+area; p<ptop;)
    {
      *p++ = (*p>>s);
      *q++ = (*q>>s);
    }
  }
}


/* constants for connect threshold make_Rij and adjust*/

#define dirNNE 01
#define dirNEE 02
#define dirES  04
#define dirSSW 010
#define dirSWW 020
#define dirWN  040

#define bearN  (dirWN|dirNNE)
#define bearE  (dirNEE|dirES)
#define bearS  (dirES|dirSSW)
#define bearW  (dirSSW|dirWN)
#define bearNE (dirNNE|dirNEE)
#define bearSW (dirSSW|dirSWW)


#define offN  (-asize)
#define offNE (1-asize)
#define offE   1
#define offS   asize
#define offSW (asize-1)
#define offW  (-1)


connect()
{
  register char *conp;
  long i;
/* initialise conflag. all 6 adjacent exist. */

for (i=0,conp=conflag;i++<area; )
    *conp++=077;


/* set up edge conditions - remove non-existent triangles over the edge */

  for (i=0, conp=conflag; i++<asize; conp+=asize) {
    *conp &= ~(dirSSW|dirSWW|dirWN);
    *(conp+asize-1) &= ~(dirNNE|dirNEE|dirES);
  }

  for (i=0, conp=conflag; i++<asize; conp++) {
    *conp &= ~(dirWN|dirNNE|dirNEE);
    *(conp+area-asize) &= ~(dirES|dirSSW|dirSWW);
  }
}

long gval(a,b,c)
long a,b,c;
{
register long t;
long g;

  t=a-b; g=t*t;
  t=b-c; g += t*t;
/*  t=c-a; g += t*t;     SKEW VERSION */

  return(g);
}

/* macros for threshold routines */
#define g_up   (gval(*p, *(p-asize), *(p-asize+1)))
#define g_down (gval(*p, *(p+asize), *(p+asize-1)))

#define gu ((g_up)   >th)
#define gd ((g_down) >th)

functional()
{
long *p;
register char *conp;
long i;
float F=0,G=0; /* Functional and sigma |E|^2 */
  for (i=0,p=x,conp=conflag;i++<area;p++,conp++) {
    if (*conp&flagU) G+=g_up;
    else if(*conp&dirNNE) F+=g_up;
    if (*conp&flagD) G+=g_down;
    else if(*conp&dirSSW) F+=g_down;
  }
  if(!sflag) fprintf(stderr,"Functional = %f, |E|^2 = %f\n",F,G);
}

old_threshold()
{
long *p;
register char *conp;
long i;
  /* set up upper/lower triangle gradient flags
     - set flag if gradient in triangle exceeds threshold */

  for (i=0,p=x,conp=conflag;i++<area;p++,conp++) {
    if ((*conp&dirNNE)&&(gu))      *conp |= flagU;
    if ((*conp&dirSSW)&&(gd))      *conp |= flagD;
  }
}

canny_threshold()
{
register char *conp;
long i;
  if((fp = fopen(canny_file_name,"r")) == NULL)
    error("can't access canny file");
  if(header||(asize<MAXCHAR))
  if(asize!=getc(fp)&0377) error("Canny file has wrong header");

  /* read in canny file into conflag array */
  for (i=0,conp=conflag;i++<area;conp++ )
    if(getc(fp)) {
      if(*conp&dirNNE) *conp |= flagU;
      if(*conp&dirSSW) *conp |= flagD;
      if(*conp&bearNE) (*(conp+offNE)) |= flagD; /* +++++ */
      if(*conp&bearE ) (*(conp+offE )) |= flagD; /* +++++ */
      if(*conp&bearN ) (*(conp+offN )) |= flagD; /* +++++ */
      if(*conp&bearS ) (*(conp+offS )) |= flagU; /* +++++ */
      if(*conp&bearSW) (*(conp+offSW)) |= flagU; /* +++++ */
      if(*conp&bearW ) (*(conp+offW )) |= flagU; /* +++++ */
  }
  fclose(fp);
}

guided_threshold()
{
long *p,*q;
register char *conp,*d;
long i;
  /* set up upper/lower triangle gradient flags
     - set flag if gradient in triangle exceeds threshold */

  for (i=0,p=x,conp=conflag;i++<area;p++,conp++) {
    if ((*conp&dirNNE)&&(gu))      *conp |= flagU;
  }

#define flagC 02

  if((fp = fopen(canny_file_name,"r")) == NULL)
    error("can't access canny file");
  if(header||(asize<MAXCHAR))
  if(asize!=getc(fp)&0377) error("Canny file has wrong header");

  for (i=0,d=Mij;i++<area; ) {  /* read in canny file into Mij array */
    if(getc(fp)) *d++ = flagC;    /* temporarily as booleans */
    else         *d++ = 0;
  }
  fclose(fp);

#define flagN 01
#define setc(bearing,off) { if(*conp&bearing) \
                                if(*(d+off)&flagC) {\
                                  (*d)|=flagN;\
                                  continue;\
                                }}

    /* make Mij array element non-zero if in 4-neighbourhood of Canny edge */
    for (i=0,conp=conflag,d=Mij;i++<area;conp++,d++ ) {
      setc(bearN,offN);
      setc(bearE,offE);
        setc(bearS,offS);
      setc(bearW,offW);
    }

    /* discard any thresholded flags not at or adjacent to Canny edge */
    for (i=0,conp=conflag,d=Mij;i++<area;conp++,d++ )
       if(!(*d&(flagC|flagN))) (*conp)&=(~flagU);  /* +++++ */

    /* synthesis Down flags (flagD) from up flags */
    for (i=0,conp=conflag;i++<area;conp++ )
      if((*conp&flagU)&&(*conp&bearNE)) (*(conp+offNE)) |= flagD; /* +++++ */
}


threshold()
{
  if(fflag) read_flags();
  else{
    if(gflag)
      guided_threshold();
    else{
      if(cflag)
        canny_threshold();
      else
        old_threshold();
    }
  }
}

/* macros for make_Rij */

#define setru2(dir,off) {if (*conp&(dir)) {\
  if (*conp&flagU) *q = *q+ *p - *(p+(off)) ;\
  (*d)++; } }

#define setrd2(dir,off) {if (*conp&(dir)) {\
  if (*conp&flagD) *q = *q+ *p - *(p+(off)) ;\
  (*d)++; } }

#define setru3(dir,off,offU) {if (*conp&(dir)) {\
  if (*(conp+(offU))&flagU) *q = *q+ *p - *(p+(off)) ;\
  (*d)++; } }

#define setrd3(dir,off,offD) {if (*conp&(dir)) {\
  if (*(conp+(offD))&flagD) *q = *q+ *p - *(p+(off)) ;\
  (*d)++; } }

#define setru4(dir,off1,off2,offU) {if (*conp&(dir)) {\
  if (*(conp+(offU))&flagU) *q = *q+ ((*p<<1)) - *(p+(off1)) - *(p+(off2));\
  (*d)+=2; } }

#define setrd4(dir,off1,off2,offD) {if (*conp&(dir)) {\
  if (*(conp+(offD))&flagD) *q = *q+ ((*p<<1)) - *(p+(off1)) - *(p+(off2));\
  (*d)+=2; } }

make_Rij()
{
long *p,*q;
register char *conp,*d;
long i;
/* setup r=div(E) in rij  and Mij array */

  for (i=0,p=x,q=rij,d=Mij,conp=conflag;i++<area;p++,q++,d++,conp++) {
    *q=0;*d=0;
    setru2(dirNNE,offN);
    setrd3(dirNEE,offE ,offNE);
    setru4(dirES ,offE ,offS ,offS );
    setrd2(dirSSW,offS);
    setru3(dirSWW,offW ,offSW);
    setrd4(dirWN ,offW ,offN ,offN );
    switch(*d) {
      case(8) : *d = 3; break ;
      case(4) : *d = 2; break ;
      case(2) : *d = 1; break ;
      default : error("Mij is bad !!!!!!!!!"); break ;
    }
  }
}

#define HALFGREY 127
#define ZEROGREY 0

write_flags()
{
register char *conp;
long i;
  if((fq = fopen(edge_file_name,"w")) == NULL)
    error("can't access flag file");
  if(header||(asize<MAXCHAR))putc(asize,fq);
  for (i=0,conp=conflag;i++<area;conp++)
    if(*conp&flagU) putc(HALFGREY,fq);
    else            putc(ZEROGREY,fq);
  for (i=0,conp=conflag;i++<area;conp++)
    if(*conp&flagD) putc(HALFGREY,fq);
    else            putc(ZEROGREY,fq);
  fclose(fq);
}

read_flags()
{
register char *conp;
long i;
  if((fp = fopen(flag_file_name,"r")) == NULL)
    error("can't access flag file");
  if(header||(asize<MAXCHAR))getc(fp);
  for (i=0,conp=conflag;i++<area;conp++)
    if ( getc(fp)==HALFGREY) *conp|=flagU;
  for (i=0,conp=conflag;i++<area;conp++)
    if ( getc(fp)==HALFGREY) *conp|=flagD;
  fclose(fp);
}

/* macros for adjust */

#define adj2(dir,off)  {if (*conp&dir) inc+=(*(p+(off)));}
#define adj3(dir,off1,off2)  {if (*conp&dir) inc+=(*(p+(off1)) + *(p+(off2)));}


/* perform one iteration of relaxation */

long adjust()
{
  long *p,*q, inc, v;
  char *conp,*d;
  long change;
  long i;

  change=0;
  for(i=0,p=x,q=rij,d=Mij,conp=conflag;i++<area;p++,conp++) {
    inc=0;
    adj2(dirNNE,offN);
    adj2(dirNEE,offE );
    adj3(dirES ,offE ,offS );
    adj2(dirSSW,offS);
    adj2(dirSWW,offW );
    adj3(dirWN ,offW ,offN );
    /* now inc is the Sum over the neigbour hood */

    v= (*p);                        /* store old lightness */

    inc = ((inc+ (*q++)) >> (*d++)); /* (Sum + rij)/Mij     */

    /* over relaxation */
                       /* N.B. pointer to function used */
    inc = (inc>>1);       /* (w)(Sum + Rij)/Mij */
    *p  = (v>>1) + inc;     /* (1-w)*lij + (w)(Sum+Rij)/Mij) */
    inc = (*p) - v;                /* new lij - old lij */
    if (inc<0)
      inc = -inc;
    if (change<inc)                /* change = max(change,|inc|) */
      change=inc;
  }/* end of for */

  /* now change is the maximum change in the whole image for this iteration */
  return(change);
}

iterate()
{
  short i,j;
  long a;

  /* report parameters used */
  if(!sflag) {
     fprintf(stderr,
    "iteration limit = %d, precision = %d, image size = %d, min change = %ld,\n"
                                               ,iter,prec,asize,minchange);
     if(cflag)
       fprintf(stderr,"canny operator output from %s used\n",canny_file_name);
     if(gflag)
       fprintf(stderr,"to guide in built edge detector\n");
     if((!cflag)&&(!fflag)) {
       if(tflag)
         fprintf(stderr,"threshold = %3.2f%%,",th_percentage);
       else
         fprintf(stderr,"threshold value = %d,",th);
     }
     fprintf(stderr,"SOR parameter = 0.5,");
     fprintf(stderr,"reporting freq. = %d\n",freq);
     
  }

  for (i=1, a=minchange+1; (i<=iter)&&(a>minchange); i++)
  {
    a=adjust();
    if ((!sflag)&&!(i % freq))
      fprintf(stderr,"%9ld%1s", a, (i%(5*freq) ? " " : "\n"));
  }
  if((!sflag)&&(i % (5*freq))) fprintf(stderr,"\n");
  if(!sflag) fprintf(stderr,"total iterations: %d\n\n",i-1);
}