/*****************************************************************************/
/*                       Image processing functions                          */
/*  Source file : image color corrections                                    */
/*  Designed by Igor Potucek (potucek@fit.vutbr.cz)                          */
/*  created 2006                                                             */
/*****************************************************************************/
#include <memory.h>
#include <math.h>

void BlendDeinterlace(unsigned char *InputImage, unsigned char *OutputImage, int Width, int Height, int Line)
// Line = 0 .... odd, Line = 1 .... even
{
    unsigned char *src = InputImage;
    unsigned char *dst = OutputImage;
    int rowsiz = Width*3;
    for (int y = 0; y < Height; y++) {
            /* If we are on a line corresponding to the 'keep', or
               if we are on an image border line (i.e. one which
               has *not* two surrounding lines), just copy it to
               the destination image.  Else, interpolate among the
               two surrounding lines. */

            if ((y % 2 == Line) || (y - 1 < 0) || (y + 1 >= Height))
            {
                memcpy(dst, src, rowsiz);
                dst+=rowsiz;
                src+=rowsiz;
            }
            else
            for (int x = 0; x<Width; x++)
            {
                        *dst++ = (*(src - rowsiz)
                                   + *(src + rowsiz)) >> 1;
                        src++;
                        *dst++ = (*(src - rowsiz)
                                   + *(src + rowsiz)) >> 1;
                        src++;
                        *dst++ = (*(src - rowsiz)
                                   + *(src + rowsiz)) >> 1;
                        src++;
            //    src+=IMAGE_OFFSET-3;
            //    dst+=IMAGE_OFFSET-3;

            } /* for */
    } /* for */
}

void BlendDeinterlaceGrey(unsigned char *InputImage, unsigned char *OutputImage, int Width, int Height, int Line)
// Line = 0 .... odd, Line = 1 .... even
{
    unsigned char *src = InputImage;
    unsigned char *dst = OutputImage;
    int rowsiz = Width;
    for (int y = 0; y < Height; y++) {
            /* If we are on a line corresponding to the 'keep', or
               if we are on an image border line (i.e. one which
               has *not* two surrounding lines), just copy it to
               the destination image.  Else, interpolate among the
               two surrounding lines. */

            if ((y % 2 == Line) || (y - 1 < 0) || (y + 1 >= Height))
            {
                memcpy(dst, src, rowsiz);
                dst+=rowsiz;
                src+=rowsiz;
            }
            else
            for (int x = 0; x<Width; x++)
            {
                        *dst++ = (*(src - rowsiz)
                                   + *(src + rowsiz)) >> 1;
                        src++;
            } /* for */
    } /* for */
}

void GrayScale(unsigned char *InputImage, unsigned char *OutputImage, int Width, int Height)
{
   unsigned char r,g,b;
   unsigned char *data = InputImage;
   // compute histograms
   for (int j=0; j<Height; j++)
     for (int i=0; i<Width; i++)
     {
        r = *data; data++;
        g = *data; data++;
        b = *data; data++;
        ////////////////////////////////////////////////////////////////////////
        int value = (0.299*r+0.587*g+0.114*b);
        if (value>255) *OutputImage = 255;
        else *OutputImage = value;
        ////////////////////////////////////////////////////////////////////////
        OutputImage++;
     }
}

/////////////////////////// RGB Bilinear Interpolation /////////////////////////
void bilinear(const unsigned char *InputImage, int Width, int Height, float X, float Y, int &r, int &g, int &b)
{ // pozor !! osetrit meze, protoze bereme i barvu 4 okolnich pixelu
  int divYY, divXX, divYX, divXY,x1,x2,y1,y2;
  if (X<1) X=1; if (Y<1) Y=1;
  if (Y>Height - 2) Y=Height - 2;
  if (X>Width - 2) X=Height - 2;

    int x=X*256;
    int y=Y*256;
    int centX=(((int)X)<<8)+(128);   // x + 0.5
    int centY=(((int)Y)<<8)+(128);   // y + 0.5
    x1=centX-(x-(128));
    x2=x+(128)-centX;
    y1=centY-(y-(128));
    y2=y+(128)-centY;
    divXX=x1*y1;
    divYY=x2*y2;
    divYX=x1*y2;
    divXY=x2*y1;

    int pos=int(Y)*Width*3+int(X)*3;
    unsigned char *data = (unsigned char *)InputImage + pos;
    r = *data*divXX+*(data+3+Width*3)*divYY+*(data+3)*divXY+*(data+Width*3)*divYX;
    g = *(data+1)*divXX+*(data+1+3+Width*3)*divYY+*(data+1+3)*divXY+*(data+1+Width*3)*divYX;
    b = *(data+2)*divXX+*(data+2+3+Width*3)*divYY+*(data+2+3)*divXY+*(data+2+Width*3)*divYX;
    r=r>>16; g=g>>16; b=b>>16;
}

void DeinterlaceByDirection(unsigned char *InputImage, unsigned char *OutputImage, int Width, int Height, float offsetx, float offsety, int Line)
// Line = 0 .... odd, Line = 1 .... even
{
   unsigned char *blended = new unsigned char[Width*Height*3];
   BlendDeinterlace(InputImage, blended, Width, Height, Line);
   unsigned char *data = InputImage;
   int rowsiz = Width*3;
  for (int j = 0; j < Height; j++ )
  {
    if (j % 2 == Line)
    {
      for (int i = 0; i < Width; i++ )
      {
         int r,g,b;
         bilinear(blended, Width, Height, i-offsetx, j-offsety, r, g, b);
         *(OutputImage)=r;
         *(OutputImage+1)=g;
         *(OutputImage+2)=b;
         OutputImage+=3;
      }
    }
    else
    {
       memcpy(OutputImage, data, rowsiz);
       OutputImage+=rowsiz;
    }
    data+=rowsiz;
  }
  delete []blended;
}

void Blending(unsigned char *InputImage, unsigned char *OutputImage, int Width, int Height)
// Line = 0 .... odd, Line = 1 .... even
{
   int rowsiz = Width*3;
   unsigned char *data = InputImage+rowsiz;
   OutputImage+=rowsiz;
  for (int j = 1; j < Height-1; j++ )
  {
      data+=3;
      OutputImage+=3;
      for (int i = 1; i < Width-1; i++ )
      {
         int r,g,b;
         r = *data*0.6+*(data-rowsiz)*0.2+*(data+rowsiz)*0.2;
         g = *(data+1)*0.6+*(data-rowsiz+1)*0.2+*(data+rowsiz+1)*0.2;
         b = *(data+2)*0.6+*(data-rowsiz+2)*0.2+*(data+rowsiz+2)*0.2;
         *(OutputImage)=r;
         *(OutputImage+1)=g;
         *(OutputImage+2)=b;
         OutputImage+=3;
         data+=3;
      }
    data+=3;
    OutputImage+=3;
  }
}

void Dilate(unsigned char* src, unsigned char *dst, int XSize, int YSize)
{
  int i,j;
  for (j = 0;j<YSize;++j)
  {
    for (i = 0;i<XSize;++i)
    {
      if ((*src)==0)
      {
        int c0=0; if (i>0)                      c0=*(src-1);
        int c1=0; if ((i>0) && (j>0))           c1=*(src-1-XSize);
        int c2=0; if (j>0)                      c2=*(src-XSize);
        int c3=0; if ((i<XSize-1)&&(j>0))       c3=*(src-XSize+1);
        int c4=0; if (i<XSize-1)                c4=*(src+1);
        int c5=0; if ((j<YSize-1) && (i<XSize-1)) c5=*(src+XSize+1);
        int c6=0; if (j<YSize-1)                c6=*(src+XSize);
        int c7=0; if ((j<YSize-1) && (i>0))     c7=*(src+XSize-1);
        if ((c0!=0) || (c1!=0) || (c2!=0) || (c3!=0) || (c4!=0) || (c5!=0) || (c6!=0) || (c7!=0))
        {
           (*dst)=255;
        }
        else
        {
           (*dst)=0;
        }
      }
      else
        (*dst)=255;
      dst += 1; src += 1;       // jump to next pixel
    }
  }
}

void Erode(unsigned char* src, unsigned char *dst, int XSize, int YSize)
{
  int i,j;
  for (j = 0;j<YSize;++j)
  {
    for (i = 0;i<XSize;++i)
    {
      if ((*src)==255)
      {
        int c0=0; if (i>0)                      c0=*(src-1);
        int c1=0; if ((i>0) && (j>0))           c1=*(src-1-XSize);
        int c2=0; if (j>0)                      c2=*(src-XSize);
        int c3=0; if ((i<XSize-1)&&(j>0))       c3=*(src-XSize+1);
        int c4=0; if (i<XSize-1)                c4=*(src+1);
        int c5=0; if ((j<YSize-1) && (i<XSize-1)) c5=*(src+XSize+1);
        int c6=0; if (j<YSize-1)                c6=*(src+XSize);
        int c7=0; if ((j<YSize-1) && (i>0))     c7=*(src+XSize-1);
        if ((c0==0) || (c1==0) || (c2==0) || (c3==0) || (c4==0) || (c5==0) || (c6==0) || (c7==0))
        {
           (*dst)=0;
        }
        else
        {
           (*dst)=255;
        }
      }
      else
        (*dst)=0;
      dst += 1; src += 1;       // jump to next pixel
    }
  }
}


/////////////////////// Gaussian Mask //////////////////////////////////////////
void GaussianMask(unsigned char* src, unsigned char *dst, int XSize, int YSize)
{
  int         MASK[5][5];
  int         c ,r , I, J;
  float       SUM;
  /* 5x5 Gaussian mask.  Ref: http://www.cee.hw.ac.uk/hipr/html/gsmooth.html */
  MASK[0][0] = 2; MASK[0][1] =  4; MASK[0][2] =  5; MASK[0][3] =  4; MASK[0][4] = 2;
  MASK[1][0] = 4; MASK[1][1] =  9; MASK[1][2] = 12; MASK[1][3] =  9; MASK[1][4] = 4;
  MASK[2][0] = 5; MASK[2][1] = 12; MASK[2][2] = 15; MASK[2][3] = 12; MASK[2][4] = 5;
  MASK[3][0] = 4; MASK[3][1] =  9; MASK[3][2] = 12; MASK[3][3] =  9; MASK[3][4] = 4;
  MASK[4][0] = 2; MASK[4][1] =  4; MASK[4][2] =  5; MASK[4][3] =  4; MASK[4][4] = 2;

   /**********************************************
   *   GAUSSIAN MASK ALGORITHM STARTS HERE
   ***********************************************/

   for(r=0; r<YSize; r++)  {
   for(c=0; c<XSize; c++)  {
        SUM = 0;
        /* image boundaries */
        if(r<2 || r>=YSize-2)
        SUM = src[c+XSize*r];
        else if(c<2 || c>=XSize-2)
        SUM = src[c+XSize*r];
        /* Convolution starts here */
        else
        {
           for(I=-2; I<=2; I++)
             for(J=-2; J<=2; J++)
              SUM = SUM + (int)( src[c+I+(r+J)*XSize]*MASK[I+2][J+2]);
           SUM=SUM/159.0;
        }
        if(SUM>255)  SUM=255;
        if(SUM<0)    SUM=0;
        *dst=(unsigned char)SUM;
        dst+=1;
   }
   }
}

//////////////////////// Sobel Edge Detection //////////////////////////////////
int Sobel(unsigned char* src, unsigned char *dst, int XSize, int YSize)
{
   int         GX[3][3];
   int         GY[3][3];
   int         I, J;
   long         sumX, sumY;
   int         SUM;
   /* 3x3 GX Sobel mask.  */
   GX[0][0] = -1; GX[0][1] = 0; GX[0][2] = 1;
   GX[1][0] = -2; GX[1][1] = 0; GX[1][2] = 2;
   GX[2][0] = -1; GX[2][1] = 0; GX[2][2] = 1;

   /* 3x3 GY Sobel mask.  */
   GY[0][0] =  1; GY[0][1] =  2; GY[0][2] =  1;
   GY[1][0] =  0; GY[1][1] =  0; GY[1][2] =  0;
   GY[2][0] = -1; GY[2][1] = -2; GY[2][2] = -1;

   for(int j=0; j<YSize; j++)  {
     for(int i=0; i<XSize; i++)  {
        sumX = 0;
        sumY = 0;
        int c0=0; if (i>0)                      c0=*(src-1);
        int c1=0; if ((i>0) && (j>0))           c1=*(src-1-XSize);
        int c2=0; if (j>0)                      c2=*(src-XSize);
        int c3=0; if ((i<XSize-1)&&(j>0))       c3=*(src-XSize+1);
        int c4=0; if (i<XSize-1)                c4=*(src+1);
        int c5=0; if ((j<YSize-1) && (i<XSize-1)) c5=*(src+XSize+1);
        int c6=0; if (j<YSize-1)                c6=*(src+XSize);
        int c7=0; if ((j<YSize-1) && (i>0))     c7=*(src+XSize-1);
         /*-------X GRADIENT APPROXIMATION------*/
         sumX +=c0*GX[1][0]; sumX +=c4*GX[1][2];
         sumX +=c3*GX[0][2]; sumX +=c1*GX[0][0];
         sumX +=c5*GX[2][2]; sumX +=c7*GX[2][0];
         /*-------Y GRADIENT APPROXIMATION-------*/
         sumY +=c1*GY[0][0]; sumY +=c3*GY[0][2];
         sumY +=c2*GY[0][1]; sumY +=c6*GY[2][1];
         sumY +=c5*GY[2][2]; sumY +=c7*GY[2][0];
         SUM = fabs((float)sumX) + fabs((float)sumY);
         SUM=(SUM>250)?255:0;
         *dst=(unsigned char)SUM;
         dst+=1;  src+=1;
     }
   }
   return 0;
}


//////////////////////// Sobel Edge Detection //////////////////////////////////
int Sobel(unsigned char* src, unsigned char *dst, int XSize, int YSize, int Thres)
{
   int         GX[3][3];
   int         GY[3][3];
   int         I, J;
   long         sumX, sumY;
   int         SUM;
   /* 3x3 GX Sobel mask.  */
   GX[0][0] = -1; GX[0][1] = 0; GX[0][2] = 1;
   GX[1][0] = -2; GX[1][1] = 0; GX[1][2] = 2;
   GX[2][0] = -1; GX[2][1] = 0; GX[2][2] = 1;

   /* 3x3 GY Sobel mask.  */
   GY[0][0] =  1; GY[0][1] =  2; GY[0][2] =  1;
   GY[1][0] =  0; GY[1][1] =  0; GY[1][2] =  0;
   GY[2][0] = -1; GY[2][1] = -2; GY[2][2] = -1;

   for(int j=0; j<YSize; j++)  {
     for(int i=0; i<XSize; i++)  {
        sumX = 0;
        sumY = 0;
        int c0=0; if (i>0)                      c0=*(src-1);
        int c1=0; if ((i>0) && (j>0))           c1=*(src-1-XSize);
        int c2=0; if (j>0)                      c2=*(src-XSize);
        int c3=0; if ((i<XSize-1)&&(j>0))       c3=*(src-XSize+1);
        int c4=0; if (i<XSize-1)                c4=*(src+1);
        int c5=0; if ((j<YSize-1) && (i<XSize-1)) c5=*(src+XSize+1);
        int c6=0; if (j<YSize-1)                c6=*(src+XSize);
        int c7=0; if ((j<YSize-1) && (i>0))     c7=*(src+XSize-1);
         /*-------X GRADIENT APPROXIMATION------*/
         sumX +=c0*GX[1][0]; sumX +=c4*GX[1][2];
         sumX +=c3*GX[0][2]; sumX +=c1*GX[0][0];
         sumX +=c5*GX[2][2]; sumX +=c7*GX[2][0];
         /*-------Y GRADIENT APPROXIMATION-------*/
         sumY +=c1*GY[0][0]; sumY +=c3*GY[0][2];
         sumY +=c2*GY[0][1]; sumY +=c6*GY[2][1];
         sumY +=c5*GY[2][2]; sumY +=c7*GY[2][0];
         SUM = fabs((float)sumX) + fabs((float)sumY);
         SUM=(SUM>Thres)?255:0;
         *dst=(unsigned char)SUM;
         dst+=1;  src+=1;
     }
   }
   return 0;
}

//////////////////////// Zero crossing Edge Detection //////////////////////////
int MexicanHat5x5(unsigned char* src, unsigned char *dst, int XSize, int YSize)
{
   int         mask[5][5]={  0,  0, -1,  0,  0,
                             0, -2, -2, -1,  0,
                            -1, -2, 16, -2, -1,
                             0, -1, -2, -1,  0,
                             0,  0, -1,  0,  0,     };
   int         I, J;
   long        SUM;

   for(int Y=0; Y<YSize; Y++)  {
   for(int X=0; X<XSize; X++)  {
        SUM = 0;
        /* image boundaries */
        if (Y<2 || Y>YSize-3)
          SUM = 0;
        else if (X<2 || X>XSize-3)
          SUM = 0;
        /* Convolution starts here */
        else
        {
         /*-------GRADIENT APPROXIMATION------*/
         for(I=-2; I<=2; I++)
         for(J=-2; J<=2; J++)
            SUM = SUM + ( src[X+I+(Y+J)*XSize] * mask[I+2][J+2]);

        }
        if (SUM<0)   SUM=0;
        if (SUM>255) SUM=255;
        *dst = (unsigned char)(SUM);
        dst+=1;
   }
   }
   return 0;
}

int MexicanHat7x7(unsigned char* src, unsigned char *dst, int XSize, int YSize, int Thres)
{
   int         mask[7][7]={  0,  0, -1, -1, -1,  0,  0,
                             0, -2, -3, -3, -3, -2,  0,
                            -1, -3,  5,  5,  5, -3, -1,
                            -1, -3,  5, 16,  5, -3, -1,
                            -1, -3,  5,  5,  5, -3, -1,
                             0, -2, -3, -3, -3, -2,  0,
                             0,  0, -1, -1, -1,  0,  0     };
   int         I, J;
   long        SUM;

   for(int Y=0; Y<YSize; Y++)  {
   for(int X=0; X<XSize; X++)  {
        SUM = 0;
        /* image boundaries */
        if (Y<3 || Y>YSize-4)
          SUM = 0;
        else if (X<3 || X>XSize-4)
          SUM = 0;
        /* Convolution starts here */
        else
        {
         /*-------GRADIENT APPROXIMATION------*/
         for(I=-3; I<=3; I++)
         for(J=-3; J<=3; J++)
            SUM = SUM + ( src[X+I+(Y+J)*XSize] * mask[I+3][J+3]);

        }
        SUM=SUM>>2;
        if (SUM>Thres)
          *dst = (unsigned char)(255);
        else
          *dst = 0;
        dst+=1;
   }
   }
   return 0;
}

int MexicanHat9x9(unsigned char* src, unsigned char *dst, int XSize, int YSize)
{
   int         mask[9][9]={  0,  0,  0, -1, -1, -1,  0,  0,  0,
                             0, -2, -3, -3, -3, -3, -3, -2,  0,
                             0, -3, -2, -1, -1, -1, -2, -3,  0,
                            -1, -3, -1,  9,  9,  9, -1, -3, -1,
                            -1, -3, -1,  9, 19,  9, -1, -3, -1,
                            -1, -3, -1,  9,  9,  9, -1, -3, -1,
                             0, -3, -2, -1, -1, -1, -2, -3,  0,
                             0, -2, -3, -3, -3, -3, -3, -2,  0,
                             0,  0,  0, -1, -1, -1,  0,  0,  0     };
   int         I, J;
   long        SUM;

   for(int Y=0; Y<YSize; Y++)  {
   for(int X=0; X<XSize; X++)  {
        SUM = 0;
        /* image boundaries */
        if (Y<4 || Y>YSize-5)
          SUM = 0;
        else if (X<4 || X>XSize-5)
          SUM = 0;
        /* Convolution starts here */
        else
        {
         /*-------GRADIENT APPROXIMATION------*/
         for(I=-4; I<=4; I++)
         for(J=-4; J<=4; J++)
            SUM = SUM + ( src[X+I+(Y+J)*XSize] * mask[I+4][J+4]);

        }
        SUM=SUM/6;
        if (SUM<0)   SUM=0;
        if (SUM>255) SUM=255;
        *dst = (unsigned char)(SUM);
        dst+=1;
   }
   }
   return 0;
}
