/*****************************************************************************
File:           fast_v2.c   November 9
Authors:        Ioan Tabus   (tabus@cs.tut.fi)
                Doina Petrescu
                Alin Alecu
		Bogdan Dumitrescu


Purpose:        Fast stack filter design


Copyright 1998   Ioan Tabus, Doina Petrescu, Alin Alecu, Bogdan Dumitrescu.
		All Rights Reserved.

Reference:
@ARTICLE{TFI,
   author = { I. T\u{a}bu\c{s} and D. Petrescu and M. Gabbouj  },
   title = {A Training Framework for  Stack and {Boolean} Filtering-{F}ast optimal design procedures and robustness case study},
   journal = TRIP # " Special Issue on Nonlinear Image Processing",
   volume = "IP-5",
   year = "1996",
   month = "June",
   pages = "809--826",
}


These programs are supplied free of charge for research purposes only,
and may not be sold or incorporated into any commercial product.  There is
ABSOLUTELY NO WARRANTY of any sort, nor any undertaking that they are
fit for ANY PURPOSE WHATSOEVER.  Use them at your own risk.  If you do
happen to find a bug, or have modifications to suggest, please report
it to  tabus@cs.tut.fi.  The copyright notice above and this statement of conditions 
must remain an integral part of each and every copy made of this file.

/****************************************************************************/
/*                                                                          */
/*                  Implementation of recursive procedure                   */ 
/*                                                                          */
/*  Inputs:                                                                 */
/*    - name of coefficients file  (e.g. as written by coeff_final.c)       */
/*    - name of output filter file                                          */
/*  Output:                                                                 */ 
/*    - filter file  (it will be written in boolean tabular form)           */
/*                                                                          */
/*  Allows insertion of: - desired window size                              */
/*                                                                          */
/*                                                                          */
/****************************************************************************/

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

#define REC_COMP_COST           /* gains are computed recursively in Hasse */


#define N0 9    /* Min. window size                                         */

#define N  17   /* Max. size of window.      Eg: 5   9   10  17             */

#define LG 131072  /* Max. no. of coefficients. Eg: 32 512 1024 131072      */

#define LN 300   /* dimension of cost matrix                                */
#define LNN 1000   

#define NUM 258064


typedef unsigned char byte;

byte fpos[LG][N+1],f[LG],fp[LG],fpocket[LG],
     ft[LG],fpinit[LG],fnon[LG];

int costs[LG]; /* Cost coefficients read from disk                       */

int cost[LG][N+1];

int C0;
double J0,J2,Jmin,J,Jbool[N+1],Jstack[N+1];

double  Rcost[LN]; /* Costs matrix, used in LP                              */

int NR,   /* Current window size                                            */
    LG1;

clock_t cl1;

double evalJc(); /* Evaluates Jstack                                        */
void comp_boool_stack(); /* Computes the Boolean and stack filter           */ 
byte comp_LP();  /* Procedure which implements LP                           */
void calculate(); /* Computes lg, ln and lnn                                */

int winsize, /* Actual window size                                          */
    lg,      /* 2^winsize                                                   */
    ln,      /* lg+2                                                        */
    lnn;     /* winsize*lg/2 + 1                                            */

byte visited[LG];

/***************************************************************************/


void calculate(int size)
{
  lg=pow(2,size);
  ln=lg+2;
  lnn=(size*lg/2) + 1;
}


/***************************************************************************/


void main( argc, argv)
int argc;
char * argv[];

{

   FILE *fpt;
   char namec[40], namef[40],str[10];


   int nn,i,j,j1,j2,jocker,depl,p,it,imin,sw,t,st,best,v[N],cn,tmp;
   int jmax,k;
   double s,pivot;


/* Input window size                                                       */
        

   printf("\n Input window size: ");
   scanf("%d",&winsize);
   calculate(winsize);

/* Reading cost coefficients from disk file                                */

    if(argc==3) strcpy(namec, argv[1]);
    else { printf("\n Name of coefficients file: ");
           scanf("%s",namec);
         }
    fpt=fopen(namec,"r");
    if(fpt==NULL) {printf("\n Can't open %s\n", namec); return;}

    if(argc==3) strcpy(namef,argv[2]);
    else { printf("\n Name of filter file: ");
           scanf("%s", namef);
         }
    for(nn=0;nn<lg;nn++)
     /* { if(fscanf(fpt,"%le",&costs[nn])!=1) */
     { if(fscanf(fpt,"%d",&costs[nn])!=1)
          { fclose(fpt); printf("\n Error reading %s\n",namec); 
            exit(-1);
          }
         Rcost[nn]=-costs[nn];  
      }



    /* fscanf(fpt,"%le",&C0); */
    fscanf(fpt,"%d",&C0); 


    fclose(fpt);

/*  Initialize cost matrix                                                 */

   cl1=clock();

    Rcost[lg]=0.;
    Rcost[lg+1]=0.;




          /***************************
         *     Main Recursions     * 
         ***************************/



    LG1=lg;

/* Load cost coefficients into 2-dimensional matrix                         */

    for(j=0;j<lg;j++) {cost[j][winsize]=costs[j];}

    for(NR=winsize-1;NR>=N0;NR--)
      {LG1=LG1/2;
      for(j=0;j<LG1;j++) {cost[j][NR]=cost[j][NR+1]+cost[j+LG1][NR+1];}
      }


    for(NR=N0;NR<=winsize;NR++)
      {

       for(j=0;j<LG1;j++) {costs[j]=cost[j][NR];}
       printf("\n *****  NR=  %d  *****\n",NR);

/* Compute boolean and stack filter                                         */

       comp_boool_stack();

       for(j=0;j<LG1;j++) {fpos[j][NR]=fp[j];fpinit[j]=fp[j]; }

/* Evaluate Jstack                                                          */
       Jstack[NR]=evalJc(fp);
       printf(" Jpos = %.5lf \n ",Jstack[NR]);
       printf(" time  %.3lf \n",((double)clock()-cl1)/CLOCKS_PER_SEC);  
       LG1=LG1*2;


      }

      LG1=lg;


/* Now try to solve LP problem                                              */
     if(comp_LP()){printf("\n LP solution could not be computed \n");}


/* Write filter to disk file                                                */

        fpt=fopen(namef,"w");
        if(fpt==NULL) {printf("\n Error writing filter\n"); return;}
        for(i=0;i<lg;i++)
           { fprintf(fpt,"%d \n",fp[i]);
           }                              
        fclose(fpt); 



 printf("\n  time  %.3lf\n",((double)clock()-cl1)/CLOCKS_PER_SEC);  

 
}

/****************************************************************************/
 
int     comp_cost_down( int nod )
{
  int c, v, l, fiu;
 
  c = costs[nod];
  visited[nod] = 1;
 
  v = 1;
  for ( l=0 ; l < NR ; l++ )
  {
    fiu = nod | v;
    if ( (fiu > nod) && ! fpocket[fiu] && ! visited[fiu] && !fp[fiu] )
      c += comp_cost_down(fiu);
    v <<= 1;
  }
 
  return c;
}
 
int     comp_cost_up( int nod )
{
  int c, v, l, tata;
 
  c = - costs[nod];
  visited[nod] = 1;
 
  v = 1;
  for ( l=0 ; l < NR ; l++ )
  {
    tata = nod & (~v);
    if ( (tata < nod) && ! fpocket[tata] && ! visited[tata] && fp[tata] )
      c += comp_cost_up(tata);
    v <<= 1;
  }
 
  return c;
}
 
/* ----------------------------------------------------------------------*/

void    reset_visited_down( int nod )
{
  int v, l, fiu;
 
  visited[nod] = 0;
 
  v = 1;
  for ( l=0 ; l < NR ; l++ )
  {
    fiu = nod | v;
    if ( (fiu > nod) && ! fpocket[fiu] )
      reset_visited_down(fiu);
    v <<= 1;
  }
}

void    reset_visited_up( int nod )
{
  int v, l, tata;
 
  visited[nod] = 0;
 
  v = 1;
  for ( l=0 ; l < NR ; l++ )
  {
    tata = nod & (~v);
    if ( (tata < nod) && ! fpocket[tata] )
      reset_visited_up(tata);
    v <<= 1;
  }
}

/* ----------------------------------------------------------------------*/
 
void    set_sons_on_one(int nod)
{
  int v, l, fiu;
 
  fp[nod] = 1;
 
  v = 1;
  for ( l=0 ; l < NR ; l++ )
  {
    fiu = nod | v;
    if ( (fiu > nod) && ! fpocket[fiu] )
      set_sons_on_one(fiu);
    v <<= 1;
  }
}

void    set_fathers_on_zero(int nod)
{
  int v, l, tata;
 
  fp[nod] = 0;
 
  v = 1;
  for ( l=0 ; l < NR ; l++ )
  {
    tata = nod & (~v);
    if ( (tata < nod) && ! fpocket[tata] )
      set_fathers_on_zero(tata);
    v <<= 1;
  }
}

/****************************************************************************/

 /* Compute the boolean and stack filter */                          
void comp_boool_stack()

{
 int i,j,j1,j2,depl,it,imin,sw,t,st,best,ifrez,Improv;
 double Jdelta,J1;
 



 /************
* Stage II  *
*************/

/* f will store decided nodes, namely f[i]=1 for cost[i]<0 and f[i]=0 for   */
/* cost[i]>0                                                                */
/* fnon will store undecided nodes                                          */
                          
 for(i=0;i<LG1;i++)
   {  fnon[i]=0;
      if(costs[i]>0.) {f[i]=0;}
          else {f[i]=1; if (costs[i]==0.)  fnon[i]=1; }
    }

/************/

/* Evaluate J                                                               */

 Jbool[NR]=evalJc(f);
   printf(" Jbool %.5lf \n ",Jbool[NR]);   


/* Test if the Boolean filter is stack                                      */


/***********************
* Stage III-1 + III-2  *
***********************/
 best=1;

 for(i=LG1-1;i>=0;i--)
   { fpocket[i]=0;fp[i]=0;

/* if f[node]=1 (decided on 1), verify all nodes that stack under. If we   */
/*  find one that has f[node']=0, then stack condition not respected       */

     if(f[i])
     { fp[i]=1;fpocket[i]=1;
       depl=1;
       for(j=0;j<NR;j++)
         { j2=(depl | i); 
           depl<<=1;
           if(j2>i) {if(!fp[j2]){fp[i]=0;fpocket[i]=0;best=0;break;} } 
                         /*j2 lays under i*/
         }
     }
   }
  

/* exit with fp=set I1                                                     */

/*******************/


/* Evaluate J                                                              */

 J0=evalJc(fp);
 printf("  J(f^+_max) %.5lf \n ",J0);

 
 if(best==0) 
   {
    printf(" Boolean isn't stack!\n");
                         

/* If Boolean isn't stack, then back to work                               */
  if  (NR!=N0) 
    { 
      for(i=0;i<(LG1/2);i++)   
           fpinit[i+(LG1/2)]=fpinit[i];
      for(i=0;i<LG1;i++)   {if(!fpocket[i]) fpinit[i]=fp[i];}
      J1=evalJc(fpinit);
      if (J1<J0)
        { for(i=0;i<LG1;i++)   {if(!fpocket[i]) fp[i]=fpinit[i];} 
        }
    }



/*******************
* Stage III-3       *
*******************/

/* for all nodes node_i                                                    */


  for(i=0;i<LG1;i++)     /* Look for compulsory fp=0 */
   {    


/* if it is decided on 0 or it is undecided, if we find a node that stacks */
/* over and is decided on 1, then clearly node_i does not belong to the    */
/* set I0                                                                  */
     if((!f[i])|(fnon[i]))
     { depl=1;fpocket[i]=1;
       for(j=0;j<NR;j++)
         { 
           j2=(~(depl) & i); 
           depl<<=1;
           if(j2<i) { if((f[j2]&(!fnon[j2]) )| (!fpocket[j2])) 
             { fpocket[i]=0;}} /* j2 is over j1  */
         }
     }
   }

  ifrez=0;

  /* Exit with fpocket = I0 */
  /*******************/

  /*ifrez will give us how many undecided nodes we have                    */
  for(i=0;i<LG1;i++) { if(fpocket[i]==0) ifrez++;}
  printf(" ifrez %d \n ",ifrez);
    it=0;



/*******************
* Stage III-5       *
*******************/


    while (1)    /* (NR==winsize) */             
      {                /* Add minterms and check for improvments */
       it++;
       J0=evalJc(fp);
       printf("  it= %d    J(improv) %.5lf ",it,J0);
       printf(" time  %.3lf  \n",((double)clock()-cl1)/CLOCKS_PER_SEC);  


      
      Improv=0;
      for(i=0;i<LG1;i++)
      {
        /* if we have an undecided node which we set to 1 */
        if ( (!fp[i]) && (!fpocket[i]) )
        {
         #ifdef REC_COMP_COST
          reset_visited_down(i);
          Jdelta = comp_cost_down( i );
          if ( Jdelta < 0 )
	  {
	    Improv=1;
            set_sons_on_one(i);
	  }
         #else
	  Jdelta=0.;
          for ( j=i ; j<LG1 ; j++ ) 
            if( ((i|j)==j) & (!fp[j]) )
	      { Jdelta+=costs[j]; } 
            if ( Jdelta < 0 )
	    {
	      Improv=1;
	      for ( j=i ; j<LG1 ; j++ )
	        if ( (i|j)==j ) 
                  { fp[j]=1; }
	    }
	 #endif
        }


        /* If we have an undecided node which we set to 0 */
        if ( (fp[i]) && (!fpocket[i]) )
	{
         #ifdef REC_COMP_COST
          reset_visited_up(i);
          Jdelta = comp_cost_up( i );
          if ( Jdelta < 0 )
	  {
	    Improv=1;
            set_fathers_on_zero(i);
	  }
         #else
          Jdelta=0.;
          for(j=0;j<=i;j++)  
            if( ((i|j)==i)&(fp[j]) )
	      { Jdelta-=costs[j]; }   
          if(Jdelta<0)
	  {
	    Improv=1;
	    for(j=0;j<=i;j++)
	      if((i|j)==i) 
                { fp[j]=0; }
	  }
	 #endif
        }

      }    /* end for(i=0.. */

      if(  (!Improv) | ( (J0-evalJc(fp))<0.0000001*J0 )  ) break;
              
    }   /* end while(NR==winsize) */


    /****************************/

  }

 else {
         for(i=0;i<LG1;i++) fp[i]=f[i];
      }
}


/***************************************************************************/

/* Evaluate J(c,cost) starting from  canonical terms                       */
double evalJc( cof)
unsigned char cof[];


{
 unsigned i1;
 double Jloc;

 Jloc=C0;
 for(i1=0;i1<LG1;i1++)
  if(cof[i1]) Jloc+=costs[i1];
 return(Jloc/NUM);
}

 

 
/***************************************************************************/
                           
/* Compute LP */
byte comp_LP()

{
   
byte  fpo[LG],cannot,best;
double costlp[LG];
char R[LNN][LN]; 
int LGG1,LNN1,LN1,
    coresp[LG],/* contains undecided nodes                                 */
    cor[LG];   /* cor[i] contains the position j for coresp[j]=node i      */
unsigned zerov[LG], /* store nonbasic variables for simplex                */
         basis[LNN]; /* store basic variables for simplex                  */
unsigned f1[LG],f2[LG];
 int nn,i,j,j1,j2,jocker,depl,p,it,tmp;
 int jmax,k;
 double s,pivot;


 
 /* Enter the simplex with fpocket=0 - undecided node                      */
 /*                        fpocket=1 - decided node                        */

 /* construct coresp and cor                                               */
LGG1=0; for(j=0;j<lg;j++)
           if(!fpocket[j]) 
               {coresp[LGG1]=j;cor[j]=LGG1;LGG1++;
                if(LGG1>=LN)   {cannot=1;return(cannot);}
               }

/* Construct R */

/* for every ni belonging to coresp, search for every node nj that stacks  */
/* under. If fpocket[nj]=1 (decided node) then R[k,i]=1 and R[k,LGG1]=1 ,  */
/* after which we select another ni                                        */
/*        If fpocket[nj]=0 (undecided node) then R[k,i]=1, R[k,j]=-1,      */
/*        R[k,LGG1]=0 after which go to next nj                            */
 LNN1=0;
 for(i=0;i<LGG1;i++)
   { j1=coresp[i];   /* j1 and i are the principal  characters  */
     jocker=1;
     depl=1;
     for(j=0;j<winsize;j++)
       { j2=depl|j1; depl<<=1;
         if(j2!=j1)   /* j2 lays under j1  */
           {  
             if(fpocket[j2])
                     {if(jocker)
                         { for(p=0;p<LGG1;p++) R[LNN1][p]=0;
                           R[LNN1][LGG1]=1;
                           R[LNN1][i]=1;
                           LNN1++;
                           if(LNN1>=LNN)   {cannot=1;return(cannot);}
                           jocker=0;
                         }
                      }
             else
               { for(p=0;p<=LGG1;p++) R[LNN1][p]=0;
                 R[LNN1][cor[j2]]=-1;
                 R[LNN1][i]=1;
                 if(LNN1>=LNN)   {cannot=1;return(cannot);}
                 LNN1++;
               }
           }
       }
   }
LN1=LGG1+2;



/* Construct zerov                                                         */
 for(it=0;it<LGG1;it++)
   zerov[it]=it;

 /* Construct basis                                                        */
 for(j=0;j<LNN1;j++)
   basis[j]=LGG1+j;


 /* Reduce fp so as to contain only decisions for ni belonging to Iu       */
 /* (undecided class). Modify Rcost in the same manner                     */
for(k=0;k<lg;k++) fpo[k]=fp[k];
for(k=0;k<LGG1;k++)  
    {
    fp[k]=fpo[coresp[k]];
    Rcost[k]=-costs[coresp[k]];
    }
 Rcost[LGG1]=0.;
 Rcost[LGG1+1]=0.;




 /* initialize simplex */
 /* for all ni belonging to undecided class, if fp[ni]=1 then find in R a   */
 /* row on which we have on column k a 1 and on the last column a 0 (the    */
 /* form fk-fj <=0 ). If we don't find any, search for a row on which we    */
 /* have on column k a 1 and on the last column a 1 (the form fk<=1)        */
 /* when we find one of these two types of rows, permute node k from zerov  */
 /* with the node from basis corresponding to the row we found.             */
 /* Add a column to R, and write 1 in this column on the position           */
 /* corresponding to that row. For all rows except the selected one, if on  */
 /* position k we have a 1, subtract from that row the initial row we found.*/
 /* If, on the other hand, we have a -1 on position k, add to  that row     */
 /* the initial row.                                                        */ 

for(k=LGG1-1;k>=0;k--)   
   { if(fp[k]==1)
       { jmax=-1;
         for(j=0;j<LNN1;j++)
           if((R[j][k]==1)&&(R[j][LGG1]==0)) {jmax=j;break;} 
         if(jmax<0)
          {  for(j=0;j<LNN1;j++)
               if((R[j][k]==1)&&(R[j][LGG1]==1)) {jmax=j;break;}
          }
        if(jmax<0) {printf("\n You are degenerate!!!\n"); return;}
        tmp=zerov[k];
        zerov[k]=basis[jmax];
        basis[jmax]=tmp;
        for(j=0;j<LNN1;j++)
            R[j][LGG1+1]=0;
        R[jmax][LGG1+1]=1;
        Rcost[LGG1+1]=0.;
        for(j=0;j<LNN1;j++)
         if(j!=jmax)
           { if(R[j][k]==1) for(i=0;i<LN1;i++) R[j][i]-=R[jmax][i];
             else if(R[j][k]==-1) for(i=0;i<LN1;i++) R[j][i]+=R[jmax][i]; 
           }
        pivot=Rcost[k];
        for(i=0;i<LN1;i++) 
                           Rcost[i]-=pivot*R[jmax][i];
        for(i=0;i<LNN1;i++) R[i][k]=R[i][LGG1+1];
        Rcost[k]=Rcost[LGG1+1];
       }
   }

/* Reconstruct the function                                                 */
  for(i=0;i<lg;i++) fp[i]=fpo[i];  

  for(i=0;i<LGG1;i++)
     if (zerov[i]<LGG1) fp[coresp[zerov[i]]]=0;
  for(i=0;i<LNN1;i++)
     if(basis[i]<LGG1) fp[coresp[basis[i]]]=R[i][LGG1];
 

  /* Evaluate J                                                             */

 printf("\n LNN1=%d  LGG1=%d  J=%.5lf \n",LNN1,LGG1,evalJc(fp));

 while(1)   /*  Optimization */
   { 
     k=0;
     s=Rcost[0];
     for(i=1;i<LGG1;i++)
       if(Rcost[i]>s) {k=i;s=Rcost[i];}
     
     if(Rcost[k]<=0) break;   /*  All done!  */

     /* identical algorithm as that of initialization */
     else 
       { jmax=-1;
         for(j=0;j<LNN1;j++)
           if((R[j][k]==1)&&(R[j][LGG1]==0)) {jmax=j;break;} 
         if(jmax<0)
          {  for(j=0;j<LNN1;j++)
               if((R[j][k]==1)&&(R[j][LGG1]==1)) {jmax=j;break;}
          }
        if(jmax<0) {printf("\n You are degenerate!!!\n"); return;}
        tmp=zerov[k];
        zerov[k]=basis[jmax];
        basis[jmax]=tmp;
        for(j=0;j<LNN1;j++)
            R[j][LGG1+1]=0;
        R[jmax][LGG1+1]=1;
        Rcost[LGG1+1]=0.;
        for(j=0;j<LNN1;j++)
         if(j!=jmax)
           { if(R[j][k]==1) for(i=0;i<LN1;i++) R[j][i]-=R[jmax][i];
             else if(R[j][k]==-1) for(i=0;i<LN1;i++) R[j][i]+=R[jmax][i]; 
           }
       pivot=Rcost[k];
       for(i=0;i<LN1;i++) Rcost[i]-=pivot*R[jmax][i];
        for(i=0;i<LNN1;i++) R[i][k]=R[i][LGG1+1];
        Rcost[k]=Rcost[LGG1+1];
       
      }
   }
   

 /* Reconstruct the function                                               */
  for(i=0;i<lg;i++) fp[i]=fpo[i];   

  for(i=0;i<LGG1;i++)
     if (zerov[i]<LGG1) fp[coresp[zerov[i]]]=0;
  for(i=0;i<LNN1;i++)
     if(basis[i]<LGG1) fp[coresp[basis[i]]]=R[i][LGG1];

  /* Evaluate J                                                            */
  printf("\n LP: LNN1=%d  LGG1=%d  J=%.5lf \n",LNN1,LGG1,evalJc(fp));


 /* Test if the  filter is stack                                           */
  best=1;
 for(i=lg-1;i>=0;i--)
   { fpocket[i]=0; 
     if(fp[i])
     {  fpocket[i]=1;
       depl=1;
       for(j=0;j<winsize;j++)
         { j2=(depl | i); 
           depl<<=1;
           if(j2>i) {if(!fp[j2]){ fpocket[i]=0;best=0;break;} } 
                 /*j2 lays under i*/
         }
     }
   }
  
if(best==0) 
    printf(" Stack is not a stack!\n");
  else
    printf(" Stack is stack!\n");


  

  printf("\n LNN1=%d  LGG1=%d  J=%.5lf \n",LNN1,LGG1,evalJc(fp));

  
  printf("  %.5lf",evalJc(fp));
  printf("   %.3lf",((double)clock()-cl1)/CLOCKS_PER_SEC);   

return(0);

}