/****************************************************/
/*This program is used to preprocess the training   */
/*data in order to insure the quality of the        */
/*training data set. For example, if we select a    */
/*set of training data and these data is used to    */
/*training for class 1, however, there will be a    */
/* number of pixels whose attributes belong to      */
/*class 2, those which will be call abberant pixels */
/* and will affect the quality of training.         */
/*This program is used to mark those abberant pixels*/
/* by assigning a lower weight (0 <weight <1). After*/
/* that we can use a thresholding technique to      */
/* filter these pixels, so as to just use the pixels*/
/* acturally belong to class 1 to enhance the       */
/*quality of training.*******************************/
/*Output:Robust_train_set_1, _2, and _3**************/
/*Note that the algorithm is published by P. Mather */
/*in 1987 13th annual conference of Remote Sensing  */
/*Society, pp 111-121.*******************************/
/****************************************************/

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

#define num_of_band 3
#define num_of_cycle 25
main(int argc, char **argv)
{
  FILE 		*fin1, *fin2, *fin3, *fout1, *fout2, *fout3;
  int   n,t,i,j,w, num_of_pixel;
  int   count1, count2, count3;
  unsigned long int a;
  char  ch,ch1;
  float num_band, threshold, distance;
  float s[16][16],is[16][16],mean_xi[16][16],minus_x[16][16],sum_of_x[16][16];
  float m[16][16], tx[16][16], sum_of_wi=0, squ_sum_wi,n1,n2,n3,n4,temp[2][2];
  float di[1000][2],fdi[1000][2], wi[1000][2], xi[16][16], p0 = 2, p1=0.5, p2 = 1.25, d0;
  unsigned char *store1, *store2, *store3;
  long address1,address2,address3;
  fpos_t *file_start1, *file_start2, *file_start3;
  file_start1 = &address1;
  file_start2 = &address2;
  file_start3 = &address3;
/*where n is the number of spectrums and c is the component number(end member)*/

   if(argc == 5)
    {

      printf("Please input robust training set threshold: ");
      scanf("%f",&threshold);
      num_of_pixel = atoi(argv[4]);


      if((fin1 = fopen(argv[1],"rb")) == NULL)/*band1 training file*/
	{
	  fprintf(stderr,"Failed to open %s\n",argv[1]);
	  exit(EXIT_FAILURE);
	}

      if((fin2 = fopen(argv[2],"rb")) == NULL)/*band2 training file*/
       {
         fprintf(stderr,"Failed to open %s\n",argv[2]);
         exit(EXIT_FAILURE);
       }

      if((fin3 = fopen(argv[3],"rb")) == NULL)/*band3 training file*/
       {
         fprintf(stderr,"Failed to open %s\n",argv[3]);
         exit(EXIT_FAILURE);
       }

      if((fout1 = fopen("robust_train_set_1","wb")) == NULL)
       {
         fprintf(stderr,"Failed to open %s\n","robust_train_set_1");
         exit(EXIT_FAILURE);
       }
      if((fout2 = fopen("robust_train_set_2","wb")) == NULL)
       {
         fprintf(stderr,"Failed to open %s\n","robust_train_set_2");
         exit(EXIT_FAILURE);
       }
      if((fout3 = fopen("robust_train_set_3","wb")) == NULL)
       {
         fprintf(stderr,"Failed to open %s\n","robust_train_set_3");
         exit(EXIT_FAILURE);
       }



      fgetpos(fin1,file_start1);
      fgetpos(fin2,file_start2);
      fgetpos(fin3,file_start3);

      store1 = (unsigned char *)malloc(2);
      store2 = (unsigned char *)malloc(2);
      store3 = (unsigned char *)malloc(2);
      cleanmatrix(xi,10,1);
      cleanmatrix(sum_of_x,10,1);
      cleanmatrix(s,10,10);
      cleanmatrix(is,10,10);
      for(count1 = 0;count1 < num_of_pixel;count1++)
	{
         fread(store1,1,1,fin1);
         fread(store2,1,1,fin2);
         fread(store3,1,1,fin3);
         sum_of_x[0][0] = sum_of_x[0][0]+store1[0];
         sum_of_x[1][0] = sum_of_x[1][0]+store2[0];
         sum_of_x[2][0] = sum_of_x[2][0]+store3[0];
	}
      mean_xi[0][0] =  sum_of_x[0][0]/num_of_pixel;
      mean_xi[1][0] =  sum_of_x[1][0]/num_of_pixel;
      mean_xi[2][0] =  sum_of_x[2][0]/num_of_pixel;

      fsetpos(fin1,file_start1);
      fsetpos(fin2,file_start2);
      fsetpos(fin3,file_start3);

      for(count1 = 0;count1 < num_of_pixel;count1++)
	{
         fread(store1,1,1,fin1);
         fread(store2,1,1,fin2);
         fread(store3,1,1,fin3);
         xi[0][0] = store1[0];
         xi[1][0] = store2[0];
         xi[2][0] = store3[0];

         minusmatrix(xi,mean_xi,3,1,minus_x);

         for(i = 0; i < num_of_band; i++)
           for(j = 0; j < num_of_band; j++)
	     {
               s[i][j] = s[i][j] + minus_x[i][0]*minus_x[j][0];
	     }
        }

      for(i = 0; i < num_of_band; i++)
          for(j = 0; j < num_of_band; j++)
	     {
               s[i][j] = s[i][j]/num_of_pixel;
	     }

      inverse(s,3,3,is);
      showmatrix(s,3,3);
      printf("\n");
      num_band = num_of_band;/*from int to float*/
      d0 = 0.75*(powf(num_band,p1) + (2/powf(p0,p1)));
      printf("===%f\n",d0 );
      for(count1= 0; count1 < num_of_cycle; count1++)
	{
         fsetpos(fin1,file_start1);
         fsetpos(fin2,file_start2);
         fsetpos(fin3,file_start3);
         sum_of_wi = 0;
         squ_sum_wi = 0;
         for(count2 = 0; count2 < num_of_pixel; count2++)/*di, fdi and wi*/
	   {
             fread(store1,1,1,fin1);
             fread(store2,1,1,fin2);
             fread(store3,1,1,fin3);
             xi[0][0] = store1[0];
             xi[1][0] = store2[0];
             xi[2][0] = store3[0];
            minusmatrix(xi,mean_xi,3,1,minus_x);
            transpose(minus_x,3,1,tx);
                               /*showmatrix(tx,1,3);*/
            multiply(tx,is,1,3,3,m);
	                       /*showmatrix(m,1,3);*/
            multiply(m,minus_x,1,3,1,temp);
            di[count2][0] = temp[0][0];
            di[count2][0] = powf(di[count2][0],p1);

            if ( di[count2][0] <= d0)
               fdi[count2][0] = di[count2][0];
            else
	      {
               n1 = di[count2][0]-d0;
               n2 = -0.5*powf(n1,p0);
               n3 = powf(p2,p0);
               n4 = n2/n3;
               fdi[count2][0] = d0*expf(n4);
	      }


            wi[count2][0] = fdi[count2][0]/di[count2][0];
            sum_of_wi = sum_of_wi + wi[count2][0];
            squ_sum_wi = squ_sum_wi + wi[count2][0]* wi[count2][0];
	   }

         printf(" %f %f\n ", sum_of_wi, squ_sum_wi);
         fsetpos(fin1,file_start1);
         fsetpos(fin2,file_start2);
         fsetpos(fin3,file_start3);
         cleanmatrix(xi,10,1);
         cleanmatrix(sum_of_x,10,1);
         cleanmatrix(s,10,10);
         cleanmatrix(is,10,10);
         for(count2 = 0; count2 < num_of_pixel; count2++)/*mean of Xi*/
	   {
             fread(store1,1,1,fin1);
             fread(store2,1,1,fin2);
             fread(store3,1,1,fin3);
             sum_of_x[0][0] = sum_of_x[0][0]+ wi[count2][0]*store1[0];
             sum_of_x[1][0] = sum_of_x[1][0]+ wi[count2][0]*store2[0];
             sum_of_x[2][0] = sum_of_x[2][0]+ wi[count2][0]*store3[0];
           }
         mean_xi[0][0] =  sum_of_x[0][0]/sum_of_wi;
         mean_xi[1][0] =  sum_of_x[1][0]/sum_of_wi;
         mean_xi[2][0] =  sum_of_x[2][0]/sum_of_wi;
	              showmatrix(mean_xi,3,1);
         fsetpos(fin1,file_start1);
         fsetpos(fin2,file_start2);
         fsetpos(fin3,file_start3);
         for(count2 = 0;count2 < num_of_pixel;count2++)/*sij*/
	   {
             fread(store1,1,1,fin1);
             fread(store2,1,1,fin2);
             fread(store3,1,1,fin3);
             xi[0][0] = store1[0];
             xi[1][0] = store2[0];
             xi[2][0] = store3[0];

             minusmatrix(xi,mean_xi,3,1,minus_x);

             for(i = 0; i < num_of_band; i++)
               for(j = 0; j < num_of_band; j++)
    	         {
                   s[i][j] = s[i][j] + wi[count2][0]*wi[count2][0]*minus_x[i][0]*minus_x[j][0];
	         }
           }

         for(i = 0; i < num_of_band; i++)
            for(j = 0; j < num_of_band; j++)
	     {
               s[i][j] = s[i][j]/(squ_sum_wi-1);
	     }

         inverse(s,3,3,is);
	 /* showmatrix(s,3,3);*/

	}

     fsetpos(fin1,file_start1);
     fsetpos(fin2,file_start2);
     fsetpos(fin3,file_start3);
     for(count2 = 0;count2 < num_of_pixel;count2++)/*wi*/
	   {
             fread(store1,1,1,fin1);
             fread(store2,1,1,fin2);
             fread(store3,1,1,fin3);
	     if(wi[count2][0] >= threshold)
	       {
                 fwrite(store1,1,1,fout1);
                 fwrite(store2,1,1,fout2);
                 fwrite(store3,1,1,fout3);
	       }
	   }

   for(count2 = 0;count2 < num_of_pixel;count2++)/*wi*/
	   {
	     if(wi[count2][0] < threshold)
                 printf("(%f,%f)",wi[count2][0], di[count2][0]);

	   }


      fclose(fin1);
      fclose(fin2);
      fclose(fin3);
      fclose(fout1);
      fclose(fout2);
      fclose(fout3);






    }
  else
      printf("\nUsage:\n\tpreprocess-training   'input band1 training file'  'input band2 training file' 'input band3 training file' 'number of training patterns for each band' .\n\n");
}


/*This function is to show matrix*/
showmatrix(z,nn,cc)
float z[16][16];
int   nn,cc;
{
  int i,j;
  printf("\n");
  printf("The matrix is:!\n");

  for(i=0; i < nn; i++)
    for(j=0; j < cc; j++)
      {
        printf("%15f",z[i][j]);
        if (j == cc-1)
           printf("\n");
      }
}



cleanmatrix(z,nn,cc)/*to clear matrix z's elements*/
float z[16][16];
int nn,cc;
{
  int i,j;

  for(i=0; i < nn ; i++)
    for(j=0; j < cc ; j++)
        z[i][j] = 0;
}/*end of cleanmatrix*/


transpose(z,nn,cc,tz)/*transpose z matrix*/
float z[16][16],tz[16][16];
int nn,cc;
{
  int i,j;

  for(i=0; i < nn ; i++)
    for(j=0; j < cc ; j++)
         tz[j][i] = z[i][j];

}/*end of transpose*/

addmatrix(z1,z2,row,col,item)/*add z1 and z2 matrix*/
float z1[16][16],z2[16][16],item[16][16];
int   row,col;
{
 int   i,j;


  for(i=0; i < row; i++)
     for(j=0; j < col; j++)
        item[i][j] = z1[i][j]+z2[i][j];

}/*end of add*/

minusmatrix(z1,z2,row,col,item)/*minus z1 and z2 matrix*/
float z1[16][16],z2[16][16],item[16][16];
int   row,col;
{
 int   i,j;


  for(i=0; i < row; i++)
     for(j=0; j < col; j++)
        item[i][j] = z1[i][j]-z2[i][j];

}/*end of add*/

multiply(z1,z2,row,col1,col2,item)/*multiply matrices z1[row][col1] and z2[col1][col2]*/
float z1[16][16],z2[16][16],item[16][16];
int row,col1,col2;
{
  int i,j,k;
  float sum;

  sum = 0;
  cleanmatrix(item,row,col2);
  for(i=0; i < row; i++)
    {
     for(j=0; j < col2; j++)
       {
        for(k=0; k < col1; k++)
	  {
            sum = sum + z1[i][k]*z2[k][j];
	  }
        item[i][j] = sum;
        sum = 0;
       }
    }

}/*end of multiply*/



/*This function is used to calculate inverse matrix of m[][]*/
/*The method is to divide original matrix into Lower and Upper matrices respectively*/
/*from the theorem: A=LU -> Inver(A)=inver(U)inver(L)*/
/*Hence, based on the theorem, the matrix A must be a square.(n = c)*/
inverse(z,nn,cc,item)
float z[16][16],item[16][16];
int nn,cc;
{
  int i,j,k,p,q,r,s;
  float l[16][16],u[16][16];
  float append,determ,sum;
  float il[16][16],iu[16][16];/*il[][] and iu[][] is the inverse matrix of l[][] and u[][]*/


  for(i=0; i < nn; i++)/*setup the elements of u[][] and l[][]*/
    for(j=0; j < cc; j++)
      {
        if(j < i)/*lower triangle elements = 0*/
          u[i][j] = 0;
        else
	  {
            u[i][j] = z[i][j];
            for(k=0; k < i ; k++)
                u[i][j] = u[i][j] - l[i][k]*u[k][j];
          }

        if(j < i)
           l[j][i] = 0;/*upper triangle elements = 0*/
        else if(j == i)/*diagonal elements = 1*/
           l[j][i] = 1;
        else
	  {
            l[j][i] = z[j][i];
            for(k=0 ; k < i ; k++)
                l[j][i] = l[j][i] - l[j][k]*u[k][i];
            l[j][i] = l[j][i]/u[i][i];
	  }
      }


  /*next to calculate the inverse of u[][] and l[][]*/
  for(i=0 ; i < nn; i++)
    for(j=0; j < cc; j++)
      {
        il[i][j] = 0;
        iu[i][j] = 0;
      }

  for(i=0; i < nn; i++)/*setup inverse matrices diagonal elements*/
    {
      il[i][i] = 1;
      if(u[i][i] != 0)
         iu[i][i] = 1/u[i][i];
      else
         iu[i][i] = 0;
    }

  for(i=0; i < cc-1; i++)/*setup L matrix rest elements*/
    {
      for(j=i+1; j < nn; j++)
        {
          il[j][i] = -l[j][i];
          for(k=i+1;k < j;k++)
	    {
              il[j][i] = il[j][i] - l[j][k]*il[k][i];
	    }

        }
    }/*L matrix end setup*/

  for(i=0; i < nn; i++)/*setup U matrix rest elements*/
    {
      for(j=i+1; j < cc; j++)
        {
          iu[i][j] = 0;
          for(k=i;k < j;k++)
	    {
              iu[i][j] = iu[i][j] - iu[i][k]*u[k][j];
	    }
          if(u[j][j] != 0)
            iu[i][j] = iu[i][j]/u[j][j];
	}
    }/*U matrix end setup*/



  /*next calculate the inverse of matrix A where A=LU,A(-1)=U(-1)L(-1)*/
  sum = 0;
  for(i=0; i < nn; i++)
    {
     for(j=0; j < cc; j++)
       {
        for(k=0; k < nn; k++)
	  {
            sum = sum +  iu[i][k]*il[k][j];
	  }
        item[i][j] = sum;
        sum = 0;
       }
    }

/*=======================================================================================*/
  /*             showmatrix(u,nn,cc);
                  showmatrix(iu,nn,cc);
                  showmatrix(l,nn,cc);
                  showmatrix(il,nn,cc);
                  showmatrix(item,nn,cc);*/
/*=======================================================================================*/

}/*end of inverse()*/