#include<bmap.h>

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

#define SIGMA 16
#define SIGMA2POW2 0.3*0.3

#define SIZE 4*SIGMA+1

#define START -((SIZE-1) / 2)

#define PI M_PI
#define MAX 255.
#define GANMA 2.2
#define GAIN 1.0

#define LAB_COND pow(6./29.,3.)

#define Xn 95.04
#define Yn 100.
#define Zn 108.89

double XYZ_M[3][3] = {
	{ 0.4124, 0.3576, 0.1805 },
	{ 0.2126, 0.7152, 0.0722 },
	{ 0.0193, 0.1192, 0.9505 }
};

double B_filter(int x,int y,int i,int j,double **Lcc){
  int k,l;

  double tmp,sum;
  double bunbo=0,bunsi=0;

  for(k=0;k<SIZE;k++){
    for(l=0;l<SIZE;l++){
      
      int tmpX = i + k + START;
      int tmpY = j + l + START;
      
      if (tmpX >= 0 && tmpX <= x-1 && tmpY >= 0 && tmpY <= y-1){
	tmp=
	  exp(-(((k+START)*(k+START)+(l+START)*(l+START))/(2.*SIGMA*SIGMA)))*
	  exp(-(pow(Lcc[i][j]-Lcc[tmpX][tmpY],2.)/(2.*SIGMA2POW2)));
	bunsi+=Lcc[tmpX][tmpY]*tmp;
	bunbo+=tmp;
      }
    }
  }   
  sum=bunsi/bunbo;
  return sum;
}

double RGB_X(double r, double g, double b) {
  return 100.0 * (XYZ_M[0][0] * r + XYZ_M[0][1] * g + XYZ_M[0][2] * b);
}

double RGB_Y(double r, double g, double b) {
  return 100.0 * (XYZ_M[1][0] * r + XYZ_M[1][1] * g + XYZ_M[1][2] * b);
}

double RGB_Z(double r, double g, double b) {
  return 100.0 * (XYZ_M[2][0] * r + XYZ_M[2][1] * g + XYZ_M[2][2] * b);
}

double LAB(double t) {
  if (t > LAB_COND){
    return pow(t, 1.0 / 3.0);
  }
  else {
    return (pow(29.0 / 3.0, 3.0)*t + 16.0) / 116.0;
  }
}

double XYZ_L(double x, double y, double z) {
  return 116.0 * LAB(y / Yn) - 16.;
}

double XYZ_a(double x, double y, double z) {
  return 500.0 * (LAB(x/Xn) - LAB(y/Yn));
}

double XYZ_b(double x, double y, double z) {
  return 200.0 * (LAB(y / Yn) - LAB(z / Zn));
}

int main(int argc,char *argv[]){
  ImageBuffer in,out;
  in=ImgLoad(argv[1]);
  out=ImgZeroNew(in->size[X],in->size[Y],in->bases,IMG_DOUBLE);  

  int i,j,k,l,m,n,s;
  double rmax,rmin,gmax,gmin,bmax,bmin;

  double LR = 0, LG = 0, LB = 0;
  double CX = 0, CY = 0, CZ = 0;
  double L  = 0, a  = 0, b  = 0;

  double count=0,pixel;
  pixel=in->size[X]*in->size[Y];

  double **Lcc;
  Lcc=(double**)malloc(sizeof(double*)*in->size[X]);
  for(m=0;m<in->size[X];m++){
    Lcc[m]=(double*)malloc(sizeof(double)*in->size[Y]);
  }
  
  double Lc[in->size[X]][in->size[Y]];

  double **Rr;
  Rr=(double**)malloc(sizeof(double*)*in->size[X]);
  for(m=0;m<in->size[X];m++){
    Rr[m]=(double*)malloc(sizeof(double)*in->size[Y]);
  }
  double **Rg;
  Rg=(double**)malloc(sizeof(double*)*in->size[X]);
  for(m=0;m<in->size[X];m++){
    Rg[m]=(double*)malloc(sizeof(double)*in->size[Y]);
  }
  double **Rb;
  Rb=(double**)malloc(sizeof(double*)*in->size[X]);
  for(m=0;m<in->size[X];m++){
    Rb[m]=(double*)malloc(sizeof(double)*in->size[Y]);
  }

  for(m=0;m<in->size[X];m++){
    for(n=0;n<in->size[Y];n++){

      LR = pow(img_e(in,m,n,R)/MAX, GANMA);
      LG = pow(img_e(in,m,n,G)/MAX, GANMA);
      LB = pow(img_e(in,m,n,B)/MAX, GANMA);
      
      CX = RGB_X(LR, LG, LB);
      CY = RGB_Y(LR, LG, LB);
      CZ = RGB_Z(LR, LG, LB);
      
      L = XYZ_L(CX, CY, CZ);
      a = XYZ_a(CX, CY, CZ);
      b = XYZ_b(CX, CY, CZ);
      
      Lcc[m][n]=L/100;
      //fprintf(stderr,"%.2lf\n",Lcc[m][n]);

    }
  }

  for(i=0;i<in->size[X];i++){
    for(j=0;j<in->size[Y];j++){

      Lc[i][j] = B_filter(in->size[X],in->size[Y],i,j,Lcc);

      if(Lc[i][j]==0)Rr[i][j] = 0;
      else{
	//yakann
	//Lc[i][j]=pow(Lc[i][j],1./2.);
      }

      //Lc[i][j]=255.*pow(Lc[i][j],1./GANMA);
      
      //img_set_e(out,i,j,R,Lc[i][j]);
      //img_set_e(out,i,j,G,Lc[i][j]);
      //img_set_e(out,i,j,B,Lc[i][j]);

      //fprintf(stderr,"(%d,%d)\t%.2lf\t",i,j,Lc[i][j]);
      //fprintf(stderr,"\n");

      if(Lc[i][j]==0)Rr[i][j] = 0;
      else Rr[i][j]=GAIN*pow(img_e(in,i,j,R)/MAX,GANMA)/Lc[i][j];

      if(Lc[i][j]==0)Rg[i][j] = 0;
      else Rg[i][j]=GAIN*pow(img_e(in,i,j,G)/MAX,GANMA)/Lc[i][j];

      if(Lc[i][j]==0)Rb[i][j] = 0;
      else Rb[i][j]=GAIN*pow(img_e(in,i,j,B)/MAX,GANMA)/Lc[i][j];

      if(Rr[i][j]>1)Rr[i][j]=1;
      if(Rr[i][j]<0)Rr[i][j]=0;
      if(Rg[i][j]>1)Rg[i][j]=1;
      if(Rg[i][j]<0)Rg[i][j]=0;
      if(Rb[i][j]>1)Rb[i][j]=1;
      if(Rb[i][j]<0)Rb[i][j]=0;

      count++;
      if((s=100.*(count/pixel))==100.*(count/pixel)){
	fprintf(stderr,"\r%.f%%",100.*(count/pixel));
      }
    }
  }

  for(i=0;i<in->size[X];i++){
    for(j=0;j<in->size[Y];j++){

      Rr[i][j]=255*pow(Rr[i][j],1./GANMA);
      Rg[i][j]=255*pow(Rg[i][j],1./GANMA);
      Rb[i][j]=255*pow(Rb[i][j],1./GANMA);

      //fprintf(stderr,"(%d,%d)\t%.2lf\t",i,j,Rr[i][j]);
      //fprintf(stderr,"(%d,%d)\t%.2lf\t",i,j,Rg[i][j]);
      //fprintf(stderr,"(%d,%d)\t%.2lf\t",i,j,Rb[i][j]);
      //fprintf(stderr,"\n");
      
      img_set_e(out,i,j,R,Rr[i][j]);
      img_set_e(out,i,j,G,Rg[i][j]);
      img_set_e(out,i,j,B,Rb[i][j]);
    }
  }

  for(m=0;m<in->size[X];m++){
    free(Lcc[m]);
  }
  free(Lcc);
  for(m=0;m<in->size[X];m++){
    free(Rr[m]);
  }
  free(Rr);
  for(m=0;m<in->size[X];m++){
    free(Rg[m]);
  }
  free(Rg);
  for(m=0;m<in->size[X];m++){
    free(Rb[m]);
  }
  free(Rb);

  ImgWrite(out,stdout);
  free(out);

  fprintf(stderr,"\n");

  return 0;
}