//=======================================================================================
//
// encoder of still images
//
/// vika.h
//
// developer: Henry Guennadi Levkin
//
//=======================================================================================

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

//=======================================================================================
typedef unsigned char byte;

//=======================================================================================
short reord4[16] =
{
 1,  2,  6,  7,
 3,  5,  8, 13,
 4,  9, 12, 14,
10, 11, 15, 16,
};

short reord8[64] =
{
 1,  2,  6,  7, 15, 16, 28, 29,
 3,  5,  8, 14, 17, 27, 30, 43,
 4,  9, 13, 18, 26, 31, 42, 44,
10, 12, 19, 25, 32, 41, 45, 54,
11, 20, 24, 33, 40, 46, 53, 55,
21, 23, 34, 39, 47, 52, 56, 61,
22, 35, 38, 48, 51, 57, 60, 62,
36, 37, 49, 50, 58, 59, 63, 64
};

//=======================================================================================
// transform matrices

short hadam4[16]=
{
1,  1,  1,  1,
1,  1, -1, -1,
1, -1, -1,  1,
1, -1,  1, -1,
};

short hadam8[64]=
{
1,  1,  1,  1,  1,  1,  1,  1,
1,  1,  1,  1, -1, -1, -1, -1,
1,  1, -1, -1, -1, -1,  1,  1,
1,  1, -1, -1,  1,  1, -1, -1,
1, -1, -1,  1,  1, -1, -1,  1,
1, -1, -1,  1, -1,  1,  1, -1,
1, -1,  1, -1, -1,  1, -1,  1,
1, -1,  1, -1,  1, -1,  1, -1,
};
      
short hadam16[256]=
{
1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,
1,  1,  1,  1,  1,  1,  1,  1, -1, -1, -1, -1, -1, -1, -1, -1,
1,  1,  1,  1, -1, -1, -1, -1, -1, -1, -1, -1,  1,  1,  1,  1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
};

//=======================================================================================
// quantazation table
int gnQuantNumer[65] =
{
   1,  12,  19,  13,  17,   3,   5,  11,
   2,  24,  19,  13,  17,   3,  10,  11,
   4,  48,  19,  26,  17,   6,  20,  22,
   8,  96,  19,  52,  34,  12,  40,  15,
  16,  35,  19,  21,  23,  25,  27,  29,
  32,  35,  38,  41,  45,  49,  53,  58,
  64,  70,  76,  83,  90,  98, 107, 117,
 128, 140, 153, 167, 181, 196, 213, 233,
 256,
};

int gnQuantDenom[65] =
{
   1,  11,  16,  10,  12,   2,   3,   6,
   1,  11,   8,   5,   6,   1,   3,   3,
   1,  11,   4,   5,   3,   1,   3,   3,
   1,  11,   2,   5,   3,   1,   3,   1,
   1,   2,   1,   1,   1,   1,   1,   1,
   1,   1,   1,   1,   1,   1,   1,   1,
   1,   1,   1,   1,   1,   1,   1,   1,
   1,   1,   1,   1,   1,   1,   1,   1,
   1,
};

//=============================================================================
double CalculateSNR(byte* pFrame1, byte* pFrame2, int nWidth, int nHeight)
{
  int i;
  short delta;
  int nSize = nWidth*nHeight;

  double fSNR = 0.;

  for(i=0; i<nSize; i++)
  {
    delta = pFrame1[i] - pFrame2[i];
    fSNR += (double)delta*delta;
  }

  fSNR  = fSNR / nSize;
  if(fSNR < 0.000001) fSNR = 0.000001;
  fSNR = 10.* log10(255. * 255. / fSNR);

  return fSNR;
}

//=======================================================================================
// arithmetic encoding
// from H.263
int gQ1 = 16384;
int gQ2 = 32768;
int gQ3 = 49152;
int gTop= 65535;

int gLow, gHigh, gOpposite_bits, gLength;
int bitCounter;

// max frequency in cumul_freq must be 16383 at first element of cumul_freq
// last element in cumul_freq must be 0
void ArithEncoder(int index, int cumul_freq[],
                  int* pBitPosition, int bitArray[], int bitArraySize)
{
 gLength = gHigh - gQ1 +1;
 gHigh = gHigh - 1+ ( gLength*cumul_freq[index])/ cumul_freq[0];
 gLow += ( gLength*cumul_freq[index+1])/ cumul_freq[0];
 
 while(1)
 {
 	if( gHigh < gQ2 )
 	{
 		// send out a bit '0'
 		bitCounter++;

 		while(gOpposite_bits > 0)
 		{
 			// send out a bit '1'
 		  bitCounter++;

 			gOpposite_bits--;
 		}
 	}

 	else if( gLow >= gQ2 )
 	{
 		// send out a bit '1'
 		bitCounter++;

 		while(gOpposite_bits > 0)
 		{
 			// send out a bit '0'
 		  bitCounter++;

 			gOpposite_bits--;
 		}
 		gLow  -= gQ2;
 		gHigh -= gQ2;
 	}

 	else if(gLow>=gQ1  && gHigh < gQ3)
 	{
 		gOpposite_bits++;
 		gLow  -= gQ1;
 		gHigh -= gQ1;
 	}

  else break;

  gLow *= 2;
  gHigh = 2*gHigh + 1;
 }
}

void ArithEncoderReset()
{
 gLow = 0;
 gHigh = gTop;
 gOpposite_bits = 0;
 
 bitCounter = 0;
}

//=======================================================================================
double EntropyCalculation(int dim, int* pHist)
{
 double fEntropy= 0.;
 double x;
 int nn = 0;
 int i;

 for(i=0; i<dim; i++)
 {
 	nn+=pHist[i];
 }

 if(nn==0) return -1.;

 for(i=0; i<dim; i++)
 {
 	if(pHist[i]==0) continue;
 	x = (double) pHist[i] / nn;
  fEntropy += (-x * log(x));
 }
 return  fEntropy/log(2.);
}
//=======================================================================================
void WritePGM(byte* imagePtr,
              int num_cols, int num_rows,
              char* file_name)
{
    FILE* img_file= fopen(file_name, "wb");

    fprintf( img_file, "P5\n#\n%d %d\n255\n", num_cols, num_rows );
    fwrite( imagePtr, 1, num_cols * num_rows, img_file);
    fclose(img_file);
}

//=======================================================================================
void GetBlockByte(byte* pImg, int nWidth, int nHeight, int xCoord, int yCoord,
                  byte* pBlock, int blkWidth, int blkHeight)
{
  int i;
  int nShift;

  if(nHeight<(yCoord+blkHeight) || nWidth<(xCoord+blkWidth)) 
  {
    printf("GetBlock: error! %d %d %d %d ", nWidth, nHeight, xCoord, yCoord);
    return;
  }

  for(i=0; i<blkHeight; i++)
  {
    nShift = (yCoord + i) * nWidth + xCoord;
    memcpy(pBlock + i*blkWidth, pImg+nShift, blkWidth);
  }
}

//=======================================================================================
void PutBlockByte(byte* pImg, int nWidth, int nHeight, int xCoord, int yCoord,
                  byte* pBlock, int blkWidth, int blkHeight)
{
  int i;
  int nShift;

  if(nHeight<(yCoord+blkHeight) || nWidth<(xCoord+blkWidth)) 
  {
    printf("PutBlock: error! %d %d %d %d ", nWidth, nHeight, xCoord, yCoord);
    return;
  }

  for(i=0; i<blkHeight; i++)
  {
    nShift = (yCoord + i) * nWidth + xCoord;
    memcpy(pImg+nShift,pBlock + i*blkWidth, blkWidth);
  }
}

//=======================================================================================
void GetBlockShort(short* pImg, int nWidth, int nHeight, int xCoord, int yCoord,
                   short* pBlock, int blkWidth, int blkHeight)
{
  int i;
  int nShift;

  if(nHeight<(yCoord+blkHeight) || nWidth<(xCoord+blkWidth)) 
  {
    printf("GetBlock: error! %d %d %d %d ", nWidth, nHeight, xCoord, yCoord);
    return;
  }

  for(i=0; i<blkHeight; i++)
  {
    nShift = (yCoord + i) * nWidth + xCoord;
    memcpy( (byte*)(pBlock + i*blkWidth), (byte*)(pImg+nShift), 2*blkWidth);
  }
}

//=======================================================================================
void PutBlockShort(short* pImg, int nWidth, int nHeight, int xCoord, int yCoord,
                   short* pBlock, int blkWidth, int blkHeight)
{
  int i;
  int nShift;

  if(nHeight<(yCoord+blkHeight) || nWidth<(xCoord+blkWidth)) 
  {
    printf("PutBlock: error! %d %d %d %d ", nWidth, nHeight, xCoord, yCoord);
    return;
  }

  for(i=0; i<blkHeight; i++)
  {
    nShift = (yCoord + i) * nWidth + xCoord;
    memcpy( (byte*)(pImg+nShift), (byte*)(pBlock + i*blkWidth), 2*blkWidth);
  }
}
//=======================================================================================
void MatrPrintByte(int dim, unsigned char *pMat)
{
  int i,j;

  puts("================================================");
  for(i=0; i<dim; i++)
  {
	  for(j=0; j<dim; j++)
	  {
      printf("%-5d ",*(pMat+i*dim+j));
	  }
    printf("\n");
  }
}

//=======================================================================================
void MatrPrintShort(int dim, short *pMat)
{
  int i,j;

  puts("================================================");
  for(i=0; i<dim; i++)
  {
	  for(j=0; j<dim; j++)
	  {
      printf("%-5d ",*(pMat+i*dim+j));
	  }
    printf("\n");
  }
}

//=======================================================================================
void FileMatrPrintByte(FILE* fp, int dim, unsigned char *pMat)
{
  int i,j;

  fputs("================================================\n", fp);
  for(i=0; i<dim; i++)
  {
	  for(j=0; j<dim; j++)
	  {
      fprintf(fp, "%-5d ",*(pMat+i*dim+j));
	  }
    fprintf(fp, "\n");
  }
}

//=======================================================================================
void FileMatrPrintShort(FILE* fp, int dim, short *pMat)
{
  int i,j;

  fputs("================================================\n", fp);
  for(i=0; i<dim; i++)
  {
	  for(j=0; j<dim; j++)
	  {
      fprintf(fp, "%-5d ",*(pMat+i*dim+j));
	  }
    fprintf(fp,"\n");
  }
}

//=======================================================================================
void MatrCopy(int dim, short* pMatr, short* pMatrCopy)
{
 int i;
 int nn = dim*dim;
 for(i=0; i<nn; i++) pMatrCopy[i] = pMatr[i];
}

//=======================================================================================
void MatrTranspos(int dim, short* pMatr, short* pMatrTranspos)
{
 int i, j;

 for(i=0; i<dim; i++)
 {
 	 for(j=0; j<dim; j++)
 	 {
 		 pMatrTranspos[i*dim+j] = pMatr[i+j*dim];
 	 }
 }
}

//=======================================================================================
void MatrMult(int dim, short *pMat1, short *pMat2, short *pMat3)
{
  int i,j,k;
  int s;

  for(i=0; i<dim; i++)
  {
    for(j=0; j<dim; j++)
    {
	    s=0;
      for(k=0;k<dim; k++)
      {
		    s += (*(pMat1+i*dim+k)) * (*(pMat2+k*dim+j));
      }
	    *(pMat3+i*dim+j) = s;
    }
  }
}

//=======================================================================================
void MatrDivideConst(int dim, short* pMat, short nNumer, short nDenom)
{
  int i;
  int n = dim*dim;

  for(i=0; i<n; i++)
  {
	    pMat[i] = ((int)pMat[i]*nDenom + nNumer/2)/ nNumer;
  }
}

//=======================================================================================
void MatrSubtract(int dim, byte* pMat1, byte* pMat2, short* pMat3)
{
  int i;
  int n = dim*dim;

  for(i=0; i<n; i++)
  {
	    pMat3[i] = (short) pMat1[i] - (short) pMat2[i];
  }
}

//=======================================================================================
void MatrAdd(int dim, byte* pMat1, short* pMat2, byte* pMat3)
{
  int i;
  int n = dim*dim;
  short r;

  for(i=0; i<n; i++)
  {
	    r = (short) pMat1[i] + pMat2[i];
	    if(r<0) pMat3[i] = 0;
	    else if(r>255) pMat3[i] =255;
	    else pMat3[i] = r;
  }
}
//=============================================================================
int CalcSAD(byte* pMatr1, byte* pMatr2, int blkDim)
{
  int i;
  int SAD = 0;

  for(i=0; i<blkDim; i++)
  {
    SAD += abs((int)pMatr1[i]-(int)pMatr2[i]);
  }

  return SAD;
}

//=======================================================================================
// read ppm image file
int PpmImageFileRead(char* filename, byte** ppPic, int* pnW, int* pnH)
{
 FILE* pInpFile;

 char buf[512];

 // open image file
 pInpFile=fopen(filename,"rb");
 if( pInpFile==NULL) return 1;
 
 fgets(buf,510,pInpFile); // "P6"
 fgets(buf,510,pInpFile); // "#comments"

 fgets(buf,510,pInpFile); // "width height"
 sscanf(buf,"%d %d", pnW, pnH);
 printf("%d %d\n", *pnW, *pnH);

 fgets(buf,510,pInpFile); // "MaxColor"

 *ppPic = malloc(3 * (*pnW) * (*pnH));
 
 fread(*ppPic, 1, (3 * (*pnW) * (*pnH)), pInpFile);

 fclose(pInpFile);

 return 0;
}
//=======================================================================================
// read ppm image file
int PpmImageFileWrite(char* filename, byte* pPic, int nWidth, int nHeight)
{
 FILE* pOutFile;

 pOutFile=fopen(filename,"wb");

 fprintf( pOutFile, "P6\n#\n%d %d\n255\n", nWidth, nHeight );
 fwrite(pPic, 1, 3 * nWidth * nHeight, pOutFile);

 fclose(pOutFile);

 return 0;
}

//=======================================================================================
// make (r,g,b) image from R,G,B components
void MakeRGBfromComponents(byte* pRGB, byte* pRed, byte* pGreen, byte* pBlue,
                           int nWidth, int nHeight)
{
 int i,ii;
 int nn = nWidth*nHeight;

 for(i=0; i<nn; i++)
 {
 	ii = i*3;
 	pRGB[ii+0]=pRed[i];
 	pRGB[ii+1]=pGreen[i];
 	pRGB[ii+2]=pBlue[i];
 }
}

//=======================================================================================
// get R,G,B components  from  (r,g,b) image
void GetComponentsFromRGB(byte* pRGB, byte* pRed, byte* pGreen, byte* pBlue,
                          int nWidth, int nHeight)
{
 int i,ii;
 int nn = nWidth*nHeight;

 for(i=0; i<nn; i++)
 {
 	ii = i*3;
 	pRed[i]   = pRGB[ii+0];
 	pGreen[i] = pRGB[ii+1];
 	pBlue[i]  = pRGB[ii+2];
 }
}

//=======================================================================================
// 4:4:4
void TransformRGBtoLuCbCr(byte* pRed,  byte* pGreen,   byte* pBlue,
                          byte* pLuma, byte* pChromaB, byte* pChromaR,
                          int nWidth, int nHeight)
{
 int i;
 int size = nWidth * nHeight;
 int acc;

 for(i=0; i<size; i++)
 {
 	acc = 0.299*pRed[i] + 0.587*pGreen[i] + 0.114* pBlue[i];
 	pLuma[i] = acc;

 	acc = 0.564*pBlue[i] - 0.564*pLuma[i] + 128.;
 	if(acc<0) acc = 0;
 	else if(acc>255) acc = 255;
 	pChromaB[i] = acc;

 	acc = 0.713*pRed[i] - 0.713*pLuma[i] + 128;
 	if(acc<0) acc = 0;
 	else if(acc>255) acc = 255;
 	pChromaR[i] = acc;
 }
}

//=======================================================================================
// 4:4:4
void TransformLuCbCrToRGB(byte* pLuma, byte* pChromaB, byte* pChromaR,
                          byte* pRed,  byte* pGreen,   byte* pBlue,
                          int nWidth, int nHeight)
{
 int i;
 int size = nWidth * nHeight;
 int acc;

 for(i=0; i<size; i++)
 {
 	acc = (double) pLuma[i] + 1.402*(pChromaR[i]-128.);
 	if(acc<0) acc = 0; 	else if(acc>255) acc = 255;
 	pRed[i] = acc;

 	acc = (double) pLuma[i] - 0.344*(pChromaB[i]-128.) - 0.714*(pChromaR[i]-128.);
 	if(acc<0) acc = 0; 	else if(acc>255) acc = 255;
 	pGreen[i] = acc;

 	acc = (double) pLuma[i] + 1.772*(pChromaB[i]-128.);
 	if(acc<0) acc = 0; 	else if(acc>255) acc = 255;
 	pBlue[i] = acc;
 }
}