//***************************LN MODEL FOR SFS******************************** **
//                    Joao L. Fernandes, Jose R.A. Torreao
//                          February 18, 2009
//******************************************************************************
//The input images should be in raw format, and, at most, 400x400 pixels in size
//The window should be, at most, 19 pixels long
//(this yields 9 discrete frequencies, by the FFTW routine employed).
//******************************************************************************

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <fftw3.h>
#include <alloc.h>

#pragma hdrstop

FILE *f1,*ref;
char num;
int Ni=256,Nj=256,NGREY=255; // Ni=number of lines; Nj=number of columns

int W=3,i,j,ir,jr,i2,j2;
// W = window size.

double sigma=3, Sigma=2.25;
// sigma=linear stage parameter (s.d. of gaussian weighting function).
// Sigma=nonlinear stage parameter (s.d. of gaussian disparity-tuning functions).
// sigma and Sigma in pixel units

int k,Wj,pj,ppi,ppj,kinicial,kfinal,zinicial,zfinal;
// Wj=number of horizontal frequency components

unsigned char *image1r;
fftw_complex *imag;
fftw_plan plano;
double pii = 3.141592654,xx[400],vzf[400][400][9],L[400][400][10];
double dx=2,jrl,irl,gaus,LM,LI,zfunc,*real,*zf;

char automatic='y',ops;

#pragma argsused

int main(int argc, char* argv[])
{
 char arq1[40],arq3[40];

 //Input of control parameters

 do
 {

  printf("=====Parameter Values=====\n");
  printf("1. Number of lines of input image = %d\n",Ni);
  printf("2. Number of columns of input image = %d\n",Nj);
  printf("3. Window size (pixels)= %d\n",W);
  printf("4. Linear stage parameter: sigma (pixels) = %f\n",sigma);
  printf("5. Nonlinear stage parameter: Sigma (pixels) = %f\n",Sigma);
  printf("Any changes <y>es or <n>o ?");
  scanf("%c",&ops);
  fflush(stdin);
  if(ops=='y')
   {
        printf("\nNumber:");
        scanf("%d",&num);
        fflush(stdin);
        switch(num) {
	   case 1: { printf("Number of lines of input image = ");
				   scanf("%d",&Ni);
				   fflush(stdin);
				   break;
				 }
		  case 2: { printf("Number of columns of input image = ");
				   scanf("%d",&Nj);
				   fflush(stdin);
				   break;
				 }
                  case 3 : { printf("W=");
				  scanf("%d",&W);
				  fflush(stdin);
				  break;
				}

		  case 4 : { printf("sigma=");
				  scanf("%lf",&sigma);
				  fflush(stdin);
				  break;
				}

                  case 5 : { printf("Sigma=");
   	        		  scanf("%lf",&Sigma);
                                  fflush(stdin);
				  break;
				}

                          }
        }
   }
   while(ops=='y');



 // Input and output files:

 printf("enter the name of the input image:");
 scanf("%s",arq1);
 printf("enter the name for the output depth matrix:");
 scanf("%s",arq3);


 // Opening file for depth map
 f1=fopen(arq3,"w");

 // Number of frequencies
 Wj=(int)(W/2)+1;

 //Parameter of the windows
 pj = (int)(W/2);


/* --------------- Matrices ----------------------*/

 image1r = (unsigned char *) malloc (Ni*Nj*sizeof(char));
 real = (double *) malloc ((W*1+1)*sizeof(double));
 zf = (double *) malloc (Ni*Nj*sizeof(double));
 imag = (fftw_complex *) fftw_malloc((Wj)*sizeof(fftw_complex));


 //Opening input image

 if ((ref = fopen(arq1,"rb")) == NULL)
  {
         printf("Can't open the image file %s\n",arq1);
         exit(1);
  }

 fread(image1r,Ni*Nj*sizeof(char),1,ref);
 fclose(ref);


 //Setting a 'metric' scale along the window

 for(j=0;j<W;j++)
  {
    xx[j] = (-dx/2.0) + ((dx/((double)(W-1)))*(double)j);
  }

 //sigma and Sigma in 'metric' units 
  sigma=sigma*dx/W;
  Sigma=Sigma*dx/W;

  for(i=0;i<Ni;i++)
    {
    for(j=0;j<Nj;j++)

      {
         zf[i*Nj+j] = 0.0;
      }
    }


// Here we start computing the Fourier transform over sliding horizontal windows
// We only need the magnitude of the transform


    for(ppi=0;ppi<(Ni);ppi=ppi+1)
     {
      for(ppj=0;ppj<(Nj);ppj=ppj+1)
      {

        plano = fftw_plan_dft_r2c_1d(W,real,imag,FFTW_ESTIMATE);

        ir=0;
        for(i=ppi;i<=ppi;i++)
         {
          jr = 0;
          for(j=ppj-pj;j<=ppj+pj;j++)
           {
            if(i<0)
             {
               kinicial = 0;
             }
            else
             {
               if(i>=Ni)
                {
                  kinicial = Ni-1;
                }
               else
                {
                  kinicial = i;
                }
             }

            if(j<0)
             {
              zinicial = 0;
             }
            else
             {
               if(j>=Nj)
                {
                  zinicial = Nj-1;
                }
              else
                {
                  zinicial = j;
                }

            }

            //Gaussian weighting function
            gaus= exp(-(((xx[jr])*(xx[jr])/(2*sigma*sigma))));
            real[ir+jr] = (((double)image1r[kinicial*Nj+zinicial])*gaus)/((double)NGREY);
            jr++;

           } // j
           ir++;
         } //  i


        fflush(stdout);
        fftw_execute(plano);

        fflush(stdout);


         for(j=0;j<Wj;j++)
          {
             //Magnitude of Fourier transform
             L[ppi][ppj][j] = (sqrt((imag[j][0]*imag[j][0])+(imag[j][1]*imag[j][1])));

             vzf[ppi][ppj][j] = 0;

          }  // Wj

      } //ppi
     } //ppj


  fftw_destroy_plan (plano); //End of Fourier transform


  //Here we start computing the depth map


  // At each window:

  for(ppi=0;ppi<(Ni);ppi=ppi+1)
   {
    for(ppj=0;ppj<(Nj);ppj=ppj+1)
     {

      //For each frequency omega:

         for(j=0;j<Wj;j++)
          {
            LI = L[ppi][ppj][j];

         //For each reference frequency Omega:

             for(j2=0;j2<Wj;j2++)
               {

                 LM = L[ppi][ppj][j2];

                 //Here we compute the nonlinear responses, based on the disparity
                 //tuning functions:

                 //For all LM
                 //Tuned inhibitory term
                 zfunc = 1.-exp(-((LI)*(LI))/(Sigma*Sigma));
                 vzf[ppi][ppj][j] = vzf[ppi][ppj][j] + zfunc;

                //Only for LM=0
                //Tuned excitatory term
                 if(LM==0)
                 {
                   zfunc = exp(-((LI)*(LI))/(Sigma*Sigma));
                   vzf[ppi][ppj][j] = vzf[ppi][ppj][j] + zfunc;
                 }

                //For all LM
                //Tuned far term (when LM not zero) or Tuned excitatory term (when LM=0)
                 {
                   zfunc = exp(-((LI-LM)*(LI-LM))/(Sigma*Sigma));
                   vzf[ppi][ppj][j] = vzf[ppi][ppj][j] + zfunc;
                 }

                //For all LM
                //Tuned near (when LM not zero) or Tuned excitatory (when LM=0)
                 {
                   zfunc = exp(-((LI+LM)*(LI+LM))/(Sigma*Sigma));
                   vzf[ppi][ppj][j] = vzf[ppi][ppj][j] + zfunc;
                 }


               }   //i,j

          }  //i2,j2

     } //ppi

   } // ppj


 // Adding the results for the various frequencies:

  for(i=0;i<Ni;i++)
   for(j=0;j<Nj;j++)
    {
        for(k=0;k<Wj;k++)
          {
             zf[i*Nj+j] = zf[i*Nj+j]+ vzf[i][j][k];
           }

     }


// Outputing the estimated depth map
// The negative sign means that we refer the depths to a reference system
// located behind the imaged surface.

 for(i=0;i<Ni;i++)
  {
    for(j=0;j<Nj;j++)
     fprintf(f1,"%d %d %f \n",i,j,(-zf[i*Nj+j]));
  fprintf(f1,"\n");
  }

 fclose(f1);
 printf("deallocation of plan was OK.\n");
 free(real);
 printf("deallocation of memory OK.\n");
 
 return 0;
}