#include "Filters.h"
#include <stdlib.h>
#include <memory.h>
#include <math.h>

#define PI 3.1415926538

unsigned char* cosCorrection(int width, int height, unsigned char* imgSrc, unsigned char* imgDest)
{
  if ((width <= 0) || (height <= 0))
    return NULL;

  unsigned char* result;
  if (!imgDest)
  {
    result = new unsigned char[3*width*height];
    if (!result)
      return NULL;
  }
  else
    result = imgDest;

  float height_cm = 18;
  float width_cm = 24.5;
  float distance = 30;
  float distPixelW = width_cm/width;
  float distPixelH = height_cm/height;

  for (int j = 0; j < height; j++)
  {
	float distj = fabs((height/2 - j))*distPixelH;
	float distj2 = distj*distj;
    for (int i = 0; i < width; i++)
	{
  	  float disti = fabs((width/2 - j))*distPixelW;
	  float disti2 = disti*disti;
	  
	  float dist = sqrt(disti2+distj2);
	  float hipotenusa = sqrt(dist*dist + distance*distance);
	  float cosangle = dist/hipotenusa;
	  float cos4 = pow(cosangle, 4);
	  for (int band = 0; band < 3; band++)
	  {
	    result[3*((j*width) + i) + band] = imgSrc[3*((j*width) + i) + band] + imgSrc[3*((j*width) + i) + band] * cos4;
	  }
	}
  }

  return result;
}

unsigned char* meanFilter(int width, int height, int neighborsRange, unsigned char* imgSrc, unsigned char* imgDest)
{
  if ((width <= 0) || (height <= 0))
    return NULL;

  unsigned char* result;
  if (!imgDest)
  {
    result = new unsigned char[3*width*height];
    if (!result)
      return NULL;
  }
  else
    result = imgDest;

  int pixelsAval = ((2*neighborsRange)+1)*((2*neighborsRange)+1);
  float div = 1.0f/(float)pixelsAval;

  // Avaliando Interior
  for (int j = 1; j < height-1; j++)
  {
    for (int i = 1; i < width-1; i++)
	{
	  int mean1=0;
	  int mean2=0;
	  int mean3=0;
	  for (int y = -neighborsRange; y <= neighborsRange; y++)
	  {
  	    for (int x = -neighborsRange; x <= neighborsRange; x++)
		{
		  mean1 = mean1 + imgSrc[3*((j+y)*width + (i+x))];	  
		  mean2 = mean2 + imgSrc[3*((j+y)*width + (i+x))+1];	  
		  mean3 = mean3 + imgSrc[3*((j+y)*width + (i+x))+2];	  
		}
	  }
	  mean1 = mean1*div;
	  mean2 = mean2*div;
	  mean3 = mean3*div;
      result[3*(j*width + i)] = (unsigned char) mean1;
      result[3*(j*width + i)+1] = (unsigned char) mean2;
      result[3*(j*width + i)+2] = (unsigned char) mean3;
	}
  }

  // Avaliando Bordas


  return result;
}

unsigned char* gaussianFilter(int width, int height, int nband, int luminance, int nPasses, unsigned char* imgSrc, unsigned char* imgDest)
{
  if ((width <= 0) || (height <= 0) || (nPasses <= 0))
    return NULL;

  unsigned char* result;
  if (!imgDest)
  {
    result = new unsigned char[3*width*height];
    if (!result)
      return NULL;
  }
  else
    result = imgDest;

  unsigned char* temp = new unsigned char[3*width*height];
  
  int kernel[9] = { 1,  2, 1,
	                2, 12, 2,
					1,  2, 1};

  float div = 1.0f/(float)24;

  for (int n = 0; n < nPasses; n++)
  {
    // Avaliando Interior
    for (int j = 1; j < height-1; j++)
    {
      for (int i = 1; i < width-1; i++)
	  {
	    for (int band = 0; band < nband; band++)
		{
		  int gaussian;
          gaussian = kernel[0]*imgSrc[3*((j-1)*width + (i-1)) + band];  // 3*((j-1)*width + (i-1))
          gaussian += kernel[1]*imgSrc[3*((j-1)*width + i) + band];     // 3*((j-1)*width + i)
          gaussian += kernel[2]*imgSrc[3*((j-1)*width + (i+1)) + band]; // 3*((j-1)*width + (i+1))

		  gaussian += kernel[3]*imgSrc[3*(j*width + (i-1)) + band]; // 3*(j*width + (i-1))
          gaussian += kernel[4]*imgSrc[3*(j*width + i) + band];     // 3*(j*width + i)
          gaussian += kernel[5]*imgSrc[3*(j*width + (i+1)) + band]; // 3*(j*width + (i+1))

		  gaussian += kernel[6]*imgSrc[3*((j+1)*width + (i-1)) + band]; // 3*((j+1)*width + (i-1))
          gaussian += kernel[7]*imgSrc[3*((j+1)*width + i) + band];     // 3*((j+1)*width + i)
          gaussian += kernel[8]*imgSrc[3*((j+1)*width + (i+1)) + band]; // 3*((j+1)*width + (i+1))

		  result[3*(j*width + i)+ band] = (unsigned char) ((float)gaussian*div);
		}
	  }
    }

	// Avaliando Bordas
	// Borda Superior


	memcpy(temp, result, 3*width*height);
	imgSrc = temp;
  }
  delete temp;
  return result;
}

unsigned char* fastGaussianFilter(int width, int height, int nband, int luminance, int nPasses, unsigned char* imgSrc, unsigned char* imgDest)
{
  if ((width <= 0) || (height <= 0) || (nPasses <= 0))
    return NULL;

  unsigned char* result;
  if (!imgDest)
  {
    result = new unsigned char[3*width*height];
    if (!result)
      return NULL;
  }
  else
    result = imgDest;

  unsigned char* temp = new unsigned char[3*width*height];

  for (int n = 0; n < nPasses; n++)
  {
    // Avaliando Interior
    // Multiplicando Colunas
    if (!luminance)
	{
      for (int j = 1; j < height-1; j++)
      {
	    int idx_j = j*width;
        for (int i = 1; i < width-1; i++) 
	    {
	      for (int band = 0; band < nband; band++)
		  {
            int gaussian = imgSrc[3*(idx_j + (i-1)) + band];      //1  -->  3*(j*width + (i-1))
            gaussian +=    imgSrc[3*(idx_j + i) + band] << 1;     //2  -->  3*(j*width + i)
            gaussian +=    imgSrc[3*(idx_j + (i+1)) + band];      //1  -->  3*(j*width + (i+1))

   	        gaussian = gaussian >> 2; // gaussian / 4
		    result[3*(idx_j + i)+ band] = (unsigned char) gaussian;
		  }
	    }
      }

	  memcpy(temp, result, 3*width*height);
	  imgSrc = temp;

	  // Multiplicando Linhas
      for (int j = 1; j < height-1; j++)
      {
	    int idx_j = j*width;
        for (int i = 1; i < width-1; i++)
	    {
	      for (int band = 0; band < nband; band++)
		  {
            int gaussian = imgSrc[3*(idx_j-width + i) + band];    //1  -->  3*((j-1)*width + i)
            gaussian +=    imgSrc[3*(idx_j + i) + band] << 1;     //2  -->  3*(j*width + i)
            gaussian +=    imgSrc[3*(idx_j+width + i) + band];    //1  -->  3*((j+1)*width + i)

		    gaussian = gaussian >> 2; // gaussian / 4
		    result[3*(idx_j + i)+ band] = (unsigned char) gaussian;
		  }
	    }
      }

	  // Avaliando Bordas
	  // Borda Superior

	  memcpy(temp, result, 3*width*height);
	  imgSrc = temp;
	}
	else
	{
    // Avaliando Interior
    // Multiplicando Colunas
      for (int j = 1; j < height-1; j++)
      {
	    int idx_j = j*width;
        for (int i = 1; i < width-1; i++) 
	    {
          int gaussian = imgSrc[3*(idx_j + (i-1))];      //1  -->  3*(j*width + (i-1))
          gaussian +=    imgSrc[3*(idx_j + i)] << 1;     //2  -->  3*(j*width + i)
          gaussian +=    imgSrc[3*(idx_j + (i+1))];      //1  -->  3*(j*width + (i+1))

   	      gaussian = gaussian >> 2; // gaussian / 4
	      for (int band = 0; band < nband; band++)
		  {
		    result[3*(idx_j + i)+ band] = (unsigned char) gaussian;
		  }
	    }
      }

	  memcpy(temp, result, 3*width*height);
	  imgSrc = temp;

	  // Multiplicando Linhas
      for (int j = 1; j < height-1; j++)
      {
	    int idx_j = j*width;
        for (int i = 1; i < width-1; i++)
	    {
          int gaussian = imgSrc[3*(idx_j-width + i)];    //1  -->  3*((j-1)*width + i)
          gaussian +=    imgSrc[3*(idx_j + i)] << 1;     //2  -->  3*(j*width + i)
          gaussian +=    imgSrc[3*(idx_j+width + i)];    //1  -->  3*((j+1)*width + i)

          gaussian = gaussian >> 2; // gaussian / 4
	      for (int band = 0; band < nband; band++)
		  {
		    result[3*(idx_j + i)+ band] = (unsigned char) gaussian;
		  }
	    }
      }

	  // Avaliando Bordas
	  // Borda Superior

	  memcpy(temp, result, 3*width*height);
	  imgSrc = temp;
	}
  }
  delete temp;
  return result;
}

int compare(const void* arg1, const void* arg2)
{
  if (*((int*) arg1) < *((int*) arg2))
    return -1;
  else if (*((int*) arg1) > *((int*) arg2))
    return 1;
  else
    return 0;
}

unsigned char* medianFilter(int width, int height, int neighborsRange, unsigned char* imgSrc, unsigned char* imgDest)
{
  if ((width <= 0) || (height <= 0))
    return NULL;

  unsigned char* result;
  if (!imgDest)
  {
    result = new unsigned char[3*width*height];
    if (!result)
      return NULL;
  }
  else
    result = imgDest;

  int* pixelsAval1 = new int[((2*neighborsRange)+1)*((2*neighborsRange)+1)];
  int* pixelsAval2 = new int[((2*neighborsRange)+1)*((2*neighborsRange)+1)];
  int* pixelsAval3 = new int[((2*neighborsRange)+1)*((2*neighborsRange)+1)];

  // Avaliando Interior
  for (int j = 1; j < height-1; j++)
  {
    for (int i = 1; i < width-1; i++)
	{
	  int count = 0;
	  for (int y = -neighborsRange; y <= neighborsRange; y++)
	  {
  	    for (int x = -neighborsRange; x <= neighborsRange; x++)
		{
          pixelsAval1[count] = imgSrc[3*((j+y)*width + (i+x))];
          pixelsAval2[count] = imgSrc[3*((j+y)*width + (i+x))+1];
          pixelsAval3[count] = imgSrc[3*((j+y)*width + (i+x))+2];
		  count++;
		}
	  }
   	  qsort(pixelsAval1, count, sizeof(int), compare);
   	  qsort(pixelsAval2, count, sizeof(int), compare);
   	  qsort(pixelsAval3, count, sizeof(int), compare);

      result[3*(j*width + i)] = pixelsAval1[4];
      result[3*(j*width + i)+1] = pixelsAval2[4];
      result[3*(j*width + i)+2] = pixelsAval3[4];
	}
  }

  // Avaliando Bordas


  delete pixelsAval1;
  delete pixelsAval2;
  delete pixelsAval3;

  return result;
}

unsigned char* conservativeFilter(int width, int height, int neighborsRange, unsigned char* imgSrc, unsigned char* imgDest)
{
  if ((width <= 0) || (height <= 0))
    return NULL;

  unsigned char* result;
  if (!imgDest)
  {
    result = new unsigned char[3*width*height];
    if (!result)
      return NULL;
  }
  else
    result = imgDest;

  // Avaliando Interior
  for (int j = 1; j < height-1; j++)
  {
    for (int i = 1; i < width-1; i++)
	{
	  for (int band = 0; band < 3; band++)
	  {
  	    unsigned char maxNeighbor = 0;
	    unsigned char minNeighbor = 255;
		unsigned char pixelValue = imgSrc[3*(j*width + i) + band];
	    for (int y = -neighborsRange; y <= neighborsRange; y++)
	    {
  	      for (int x = -neighborsRange; x <= neighborsRange; x++)
		  {
		    // Somente se nao for o ponto central (ou seja, o pixel em questao)
		    if ( x != 0 || y != 0)
			{
		      unsigned char currValue = imgSrc[3*((j+y)*width + (i+x)) + band];
		      if (currValue > maxNeighbor)
			    maxNeighbor = currValue;
			  if (currValue < minNeighbor)
				  minNeighbor = currValue;				
			}
		  }
	    }
		if (pixelValue > maxNeighbor)
          result[3*(j*width + i) + band] = maxNeighbor;
		else if (pixelValue < minNeighbor)
          result[3*(j*width + i) + band] = minNeighbor;
		else
          result[3*(j*width + i) + band] = pixelValue;
	  }
	}
  }

  // Avaliando Bordas

  return result;
}

unsigned char* diffFilter(int width, int height, unsigned char* imgSrc1, unsigned char* imgSrc2, unsigned char* imgDest)
{
  if ((width <= 0) || (height <= 0))
    return NULL;

  unsigned char* result;
  if (!imgDest)
  {
    result = new unsigned char[3*width*height];
    if (!result)
      return NULL;
  }
  else
    result = imgDest;

  unsigned char* p1 = imgSrc1;
  unsigned char* p2 = imgSrc2;
  unsigned char* p3 = result;
  for (int i = 0; i < 3*width*height; i++)
  {
    int temp = *p2 - *p1;
	if (temp < 0)
	  temp = 0;
	if (temp > 255)
	  temp = 255;
    *p3 = (unsigned char) temp;
	++p1; ++p2; ++p3;
  }

  return result;
}
/*
unsigned char* diffFilterNormalized(int width, int height, unsigned char* imgSrc1, unsigned char* imgSrc2, unsigned char* imgDest)
{
  if ((width <= 0) || (height <= 0))
    return NULL;

  unsigned char* result;
  if (!imgDest)
  {
    result = new unsigned char[3*width*height];
    if (!result)
      return NULL;
  }
  else
    result = imgDest;

  int* temp_p = new int[3*width*height];

  unsigned char* p1 = imgSrc1;
  unsigned char* p2 = imgSrc2;
  int* p3 = temp_p;

  int max = 0;
  int min = 255;

  for (int i = 0; i < 3*width*height; i++)
  {
    int temp = (int)(*p2 - *p1);
	if (temp > max)
	  max = temp;
	if (temp < min)
	  min = temp;
    *p3 = temp;
	++p1; ++p2; ++p3;
  }

  p3 = temp_p;
  unsigned char* p4 = result;
  float div = 255.0f/(float)(max - min);
  for (int i = 0; i < 3*width*height; i++)
  {
    int temp = (*p3 - min);
    *p4 = (unsigned char) ((temp) * div);
	++p4; ++p3;
  }
  delete temp_p;

  return result;
}
*/
unsigned char* diffFilterNormalized(int width, int height, unsigned char* imgSrc1, unsigned char* imgSrc2, unsigned char* imgDest)
{
  if ((width <= 0) || (height <= 0))
    return NULL;

  unsigned char* result;
  if (!imgDest)
  {
    result = new unsigned char[3*width*height];
    if (!result)
      return NULL;
  }
  else
    result = imgDest;

  int* temp_p = new int[3*width*height];

  int max = 0;
  int min = 255;
  for (int j = 1; j < height-1; j++)
  {
    for (int i = 1; i < width -1; i++)
	{
	  for (int band = 0; band < 3; band++)
	  {
		int temp = (int)(imgSrc1[3*(j*width + i) + band] - imgSrc2[3*(j*width + i) + band]);
  	    if (temp > max)
	      max = temp;
	    if (temp < min)
	      min = temp;
        temp_p[3*(j*width + i) + band] = temp;

	  }
	}
  }

  float div = 255.0f/(float)(max - min);
  for (int j = 1; j < height-1; j++)
  {
    for (int i = 1; i < width -1; i++)
	{
	  for (int band = 0; band < 3; band++)
	  {
       int temp = (temp_p[3*(j*width + i) + band] - min);
       result[3*(j*width + i) + band] = (unsigned char) ((temp) * div);
	  }
	}
  }
  delete temp_p;

  return result;
}