#include <stdlib.h>
#include <string.h>

#include "filters.h"

void hideBackRGBDiff(unsigned char* img, unsigned char* bmask, unsigned char usePrevMask, unsigned char* avg, int width, int height)
{
  int size = width*height*3;
  int i;
  int diff;
  unsigned char img_r, img_g, img_b;
  unsigned char avg_r, avg_g, avg_b;
  for(i=0; i<size; i+=3)
  {
    img_r = img[i+2];
    img_g = img[i+1];
    img_b = img[i  ];
    avg_r = avg[i+2];
    avg_g = avg[i+1];
    avg_b = avg[i  ];
	diff = (img_r-avg_r)^2;
	diff += (img_g-avg_g)^2;
	diff += (img_b-avg_b)^2;
	if(abs(diff) < 36)
	{
      bmask[i  ] = 0;
      bmask[i+1] = 0;
      bmask[i+2] = 0;
	} else
	{
	  if(!usePrevMask)
	  {
        bmask[i  ] = 255;
        bmask[i+1] = 255;
        bmask[i+2] = 255;
	  }
	}
  }
}

void hideBackNormRGBDiff(unsigned char* img, unsigned char* bmask, unsigned char usePrevMask, unsigned char* avg, int width, int height)
{
  int size = width*height*3;
  int i;
  double diff;
  double img_rgb, img_r, img_g, img_b;
  double avg_rgb, avg_r, avg_g, avg_b;
  for(i=0; i<size; i+=3)
  {
	img_rgb = img[i] + img[i+1] + img[i+2];
    img_r = ((double)img[i+2]) / img_rgb;
    img_g = ((double)img[i+1]) / img_rgb;
    img_b = ((double)img[i  ]) / img_rgb;
	avg_rgb = avg[i] + avg[i+1] + avg[i+2];
    avg_r = ((double)avg[i+2]) / avg_rgb;
    avg_g = ((double)avg[i+1]) / avg_rgb;
    avg_b = ((double)avg[i  ]) / avg_rgb;
	diff = ((img_r-avg_r) > 0) ? (img_r-avg_r) : (avg_r-img_r);
	diff += ((img_g-avg_g) > 0) ? (img_g-avg_g) : (avg_g-img_g);
	diff += ((img_b-avg_b) > 0) ? (img_b-avg_b) : (avg_b-img_b);
	if(diff < 0.06)
	{
      bmask[i  ] = 0;
      bmask[i+1] = 0;
      bmask[i+2] = 0;
	} else
	{
	  if(!usePrevMask)
	  {
        bmask[i  ] = 255;
        bmask[i+1] = 255;
        bmask[i+2] = 255;
	  }
	}
  }
}

void hideBackElgammalsrg(unsigned char* img, unsigned char* bmask, unsigned char usePrevMask, unsigned char* avg, double* avg_norm, double* std_dev_norm, int width, int height)
{
  int size = width*height*3;
  int i, count;
  double diff;
  double img_s, img_r, img_g;
  double avg_s, avg_r, avg_g;
  double alpha = 0.5;
  double beta = 1.0/alpha;
  double factor = 4.5;

  for(i=0; i<size; i+=3)
  {
	img_s = img[i] + img[i+1] + img[i+2];
	avg_s = avg[i] + avg[i+1] + avg[i+2];
    img_r = ((double)img[i+2]) / img_s;
    img_g = ((double)img[i+1]) / img_s;
    avg_r = (double)avg_norm[i+2];
    avg_g = (double)avg_norm[i+1];
	count = 0;
//	diff = ((img_r-avg_r) > 0) ? (img_r-avg_r) : (avg_r-img_r);
//	if(diff > factor*std_dev_norm[i]) count++;
//	diff = ((img_g-avg_g) > 0) ? (img_g-avg_g) : (avg_g-img_g);
//	if(diff > factor*std_dev_norm[i+1]) count++;
	diff = (img_r-avg_r)*(img_r-avg_r) + (img_g-avg_g)*(img_g-avg_g);

//	if((count < 1)&&(img_s/avg_s > alpha)&&(img_s/avg_s < beta))
	if((diff < 0.0008)&&(img_s/avg_s > alpha)&&(img_s/avg_s < beta))
	{
      bmask[i  ] = 0;
      bmask[i+1] = 0;
      bmask[i+2] = 0;
	} else
	{
	  if(!usePrevMask)
	  {
        bmask[i  ] = 255;
        bmask[i+1] = 255;
        bmask[i+2] = 255;
	  }
	}
  }
}

void hideImgBackground2(unsigned char* img, unsigned char* avg, int width, int height)
{
  int size = width*height*3;
  int i, j, index;
  int diff, iaux;
  unsigned char img_r, img_g, img_b;
  unsigned char avg_r, avg_g, avg_b;
  for(i=0; i<width; i++)
  {
    for(j=0; j<height; j++)
	{
      index = (j*width + i)*3;
      img_r = img[index+2];
      img_g = img[index+1];
      img_b = img[index  ];
      avg_r = avg[index+2];
      avg_g = avg[index+1];
      avg_b = avg[index  ];
	  iaux = img_r + img_g + img_b;
	  iaux -= avg_r + avg_g + avg_b;
	  diff = abs(iaux);
	  if(i>0)
	  {
        avg_r = avg[index-3];
        avg_g = avg[index-2];
        avg_b = avg[index-1];        
	    iaux = img_r + img_g + img_b;
	    iaux -= avg_r + avg_g + avg_b;
	    if(diff>abs(iaux)) diff = abs(iaux);
	  }
	  if(i<(width-1))
	  {
        avg_r = avg[index+3];
        avg_g = avg[index+4];
        avg_b = avg[index+5];        
	    iaux = img_r + img_g + img_b;
	    iaux -= avg_r + avg_g + avg_b;
	    if(diff>abs(iaux)) diff = abs(iaux);
	  }
	  if(j>0)
	  {
        avg_r = avg[index-width  ];
        avg_g = avg[index-width+1];
        avg_b = avg[index-width+2];        
	    iaux = img_r + img_g + img_b;
	    iaux -= avg_r + avg_g + avg_b;
	    if(diff>abs(iaux)) diff = abs(iaux);
	  }
	  if(j<(height-1))
	  {
        avg_r = avg[index+width  ];
        avg_g = avg[index+width+1];
        avg_b = avg[index+width+2];        
	    iaux = img_r + img_g + img_b;
	    iaux -= avg_r + avg_g + avg_b;
	    if(diff>abs(iaux)) diff = abs(iaux);
	  }
	  if(abs(diff) < 30)
	  {
        img[index  ] = 255;
        img[index+1] = 255;
        img[index+2] = 255;
	  }
	}
  }
}

/*
 *      r   X
 *  M * g = Y
 *      b   Z
 *
 *  0.41  0.36  0.18
 *  0.21  0.71  0.07
 *  0.02  0.12  0.95
 */
void hideImgBackground3(unsigned char* img, unsigned char* avg, int width, int height)
{
  int size = width*height*3;
  int i;
  double diff;
  unsigned char img_r, img_g, img_b;
  unsigned char avg_r, avg_g, avg_b;
  double x1, y1, X1, Y1, Z1, XYZ1;
  double x2, y2, X2, Y2, Z2, XYZ2;
  for(i=0; i<size; i+=3)
  {
    img_r = img[i];
    img_g = img[i+1];
    img_b = img[i+2];
    avg_r = avg[i  ];
    avg_g = avg[i+1];
    avg_b = avg[i+2];
	X1 = img_r * 0.41 + img_g * 0.36 + img_b * 0.18;
	Y1 = img_r * 0.21 + img_g * 0.71 + img_b * 0.07;
	Z1 = img_r * 0.02 + img_g * 0.12 + img_b * 0.95;
    XYZ1 = X1 + Y1 + Z1;
	x1 = X1 / XYZ1;
	y1 = Y1 / XYZ1;
	X2 = avg_r * 0.41 + avg_g * 0.36 + avg_b * 0.18;
	Y2 = avg_r * 0.21 + avg_g * 0.71 + avg_b * 0.07;
	Z2 = avg_r * 0.02 + avg_g * 0.12 + avg_b * 0.95;
    XYZ2 = X2 + Y2 + Z2;
	x2 = X2 / XYZ2;
	y2 = Y2 / XYZ2;
//	diff = x1 + y1 - x2 - y2;
	diff = X1 + Y1 + Z1 - X2 - Y2 - Z2;
	if((diff < 50)&&(diff > -50))
	{
      img[i  ] = 255;
      img[i+1] = 255;
      img[i+2] = 255;
	}
  }
}

/*
 *  Y  =  0.257*R + 0.504*G + 0.098*B + 16
 *  Cr =  0.439*R - 0.368*G + 0.071*B + 128
 *  Cb = -0.148*R - 0.291*G + 0.439*B + 128
 *
 */
void hideBackHueDiff(unsigned char* img, unsigned char* bmask, unsigned char usePrevMask, unsigned char* avg, int width, int height)
{
  int size = width*height*3;
  int i;
  double diff;
  unsigned char img_r, img_g, img_b;
  unsigned char avg_r, avg_g, avg_b;
  double Cr1, Cb1;
  double Cr2, Cb2;
  for(i=0; i<size; i+=3)
  {
    img_r = img[i+2];
    img_g = img[i+1];
    img_b = img[i  ];
    avg_r = avg[i+2];
    avg_g = avg[i+1];
    avg_b = avg[i  ];
	Cr1 = img_r * 0.439 - img_g * 0.368 + img_b * 0.071 + 128;
	Cb1 = img_r * -0.148 - img_g * 0.291 + img_b * 0.439 + 128;
	Cr2 = avg_r * 0.439 - avg_g * 0.368 + avg_b * 0.071 + 128;
	Cb2 = avg_r * -0.148 - avg_g * 0.291 + avg_b * 0.439 + 128;
	diff = (Cr1 - Cr2)*(Cr1 - Cr2) + (Cb1 - Cb2)*(Cb1 - Cb2);
	if(diff < 49)
	{
      bmask[i  ] = 0;
      bmask[i+1] = 0;
      bmask[i+2] = 0;
	} else
	{
	  if(!usePrevMask)
	  {
        bmask[i  ] = 255;
        bmask[i+1] = 255;
        bmask[i+2] = 255;
	  }
	}
  }
}

void hideBackRGBDiffHueDiff(unsigned char* img, unsigned char* bmask, unsigned char usePrevMask, unsigned char* avg, int width, int height)
{
  int size = width*height*3;
  int i;
  int diff;
  unsigned char img_r, img_g, img_b;
  unsigned char avg_r, avg_g, avg_b;
  double Y1, Cr1, Cb1;
  double Y2, Cr2, Cb2;
  double Ydiff, Crdiff;
  for(i=0; i<size; i+=3)
  {
    img_r = img[i+2];
    img_g = img[i+1];
    img_b = img[i  ];
    avg_r = avg[i+2];
    avg_g = avg[i+1];
    avg_b = avg[i  ];
	diff = (img_r-avg_r)^2;
	diff += (img_g-avg_g)^2;
	diff += (img_b-avg_b)^2;
	if(abs(diff) < 30)
	{
      img[i  ] = 255;
      img[i+1] = 255;
      img[i+2] = 255;
	} else
	{
	  Y1  = img_r * 0.257 + img_g * 0.504 + img_b * 0.098 + 16;
	  Cr1 = img_r * 0.439 - img_g * 0.368 + img_b * 0.071 + 128;
	  Cb1 = img_r * -0.148 - img_g * 0.291 + img_b * 0.439 + 128;
	  Y2  = avg_r * 0.257 + avg_g * 0.504 + avg_b * 0.098 + 16;
	  Cr2 = avg_r * 0.439 - avg_g * 0.368 + avg_b * 0.071 + 128;
	  Cb2 = avg_r * -0.148 - avg_g * 0.291 + avg_b * 0.439 + 128;
	  Ydiff = (Y1 - Y2)*(Y1 - Y2);
	  Crdiff = (Cr1 - Cr2)*(Cr1 - Cr2) + (Cb1 - Cb2)*(Cb1 - Cb2);
	  if(Crdiff < 80)
	  {
        img[i  ] = 255;
        img[i+1] = 255;
        img[i+2] = 255;
	  }
	}
  }
}

/*
 *
 */
double alpha = 5.5;
double beta = 4.06;
int c1 = 3;
int c2 = 2;

void hideBackHongRGB(unsigned char* img, unsigned char* bmask, unsigned char usePrevMask, unsigned char* avg, double* std_dev, int width, int height)
{
  int size = width*height*3;
  int i, count;
  unsigned char img_r, img_g, img_b;
  unsigned char avg_r, avg_g, avg_b;
  for(i=0; i<size; i+=3)
  {
	count = 0;
    img_r = img[i+2];
    img_g = img[i+1];
    img_b = img[i  ];
    avg_r = avg[i+2];
    avg_g = avg[i+1];
    avg_b = avg[i  ];
	if((double)abs(img_r-avg_r) < alpha*std_dev[i  ]) count++;
	if((double)abs(img_g-avg_g) < alpha*std_dev[i+1]) count++;
	if((double)abs(img_b-avg_b) < alpha*std_dev[i+2]) count++;
	if(count >= c1)
	{
      bmask[i  ] = 0;
      bmask[i+1] = 0;
      bmask[i+2] = 0;
	} else
	{
	  if(!usePrevMask)
	  {
        bmask[i  ] = 255;
        bmask[i+1] = 255;
        bmask[i+2] = 255;
	  }
	}
  }
}

void hideBackHongrgb(unsigned char* img, unsigned char* bmask, unsigned char usePrevMask, double* avg_norm, double* std_dev_norm, int width, int height)
{
  int size = width*height*3;
  int i, count;
  double img_r, img_g, img_b, rgb;
  double avg_r, avg_g, avg_b;
  double diff_r, diff_g, diff_b;
  for(i=0; i<size; i+=3)
  {
	count = 0;
	rgb = img[i] + img[i+1] + img[i+2];
    img_r = ((double)img[i+2])/rgb;
    img_g = ((double)img[i+1])/rgb;
    img_b = ((double)img[i  ])/rgb;
    avg_r = avg_norm[i+2];
    avg_g = avg_norm[i+1];
    avg_b = avg_norm[i  ];
	diff_r = ((img_r-avg_r) > 0) ? (img_r-avg_r) : (avg_r-img_r);
	diff_g = ((img_g-avg_g) > 0) ? (img_g-avg_g) : (avg_g-img_g);
	diff_b = ((img_b-avg_b) > 0) ? (img_b-avg_b) : (avg_b-img_b);
	if(diff_r < beta*std_dev_norm[i+2]) count++;
	if(diff_g < beta*std_dev_norm[i+1]) count++;
	if(diff_b < beta*std_dev_norm[i  ]) count++;
	if(count >= c2)
	{
      bmask[i  ] = 0;
      bmask[i+1] = 0;
      bmask[i+2] = 0;
	} else
	{
	  if(!usePrevMask)
	  {
        bmask[i  ] = 255;
        bmask[i+1] = 255;
        bmask[i+2] = 255;
	  }
	}
  }
}