package descriptors;

import java.awt.image.BufferedImage;
import java.io.File;

import javax.imageio.ImageIO;

import preprocessing.Util;

public class HoG {
	
	public final static int MAX_VOTE = 10;
	
	/*public final static int numCellsX = 1;
	
	public final static int numCellsY = 1;*/

	/*HOG Jonathan*/
	
	private double[] img_grad_X (int[] img_src, int width, int height)
    {
            int l = width*height;
            double[] grad_X = new double[l];
            for(int i = 1; i < l-1; i++) {
                    grad_X[i]=img_src[i+1]-img_src[i-1];
            }
            return grad_X;
    }

	private double[] img_grad_Y (int[] img_src, int width, int height)
	{
	        int l = width*height;
	        double[] grad_Y = new double[l];
	        for(int i = width; i< (l - width);i++) {
	        	grad_Y[i] = img_src[i+width]-img_src[i-width];
	        }
	        return grad_Y;
	}

	private double[] module_grad (double[] grad_X, double[] grad_Y, int sx, int sy)
	{
	        int i;
	        int l=sx*sy;
	        double[] norm = new double[l];
	        for(i=sx;i<(l-sx);i++)
	                norm[i]=Math.sqrt(grad_X[i]*grad_X[i]+grad_Y[i]*grad_Y[i]);
	        return norm;
	}

	private double[] argument_grad (double[] grad_X, double[] grad_Y, int sx, int sy)
	{
	        int i;
	        int l=sx*sy;
	
	        double[] angle = new double[l];
	        
	        for(i=sx;i<l-sx;i++) {
	            angle[i]= Math.atan2(grad_Y[i], grad_X[i]);
	        }
	        return angle;
	}
	
	public double[] HoGJonathan (BufferedImage inimg)
	{
		boolean debugHoG = false;
		
		int width  = inimg.getWidth(null);
		
		int height = inimg.getHeight(null);
		
		int[] inarray = Util.getImageArray(inimg);
		
		/*Image X derivative*/
		double[] grad_X = img_grad_X (inarray, width, height);
		
		/*Image Y derivative*/
		double[] grad_Y = img_grad_Y (inarray, width, height);
		
		/*Image Norm*/
		double[] norm = module_grad(grad_X, grad_Y, width, height);
		
		/*Image Angle*/
		double[] angle = argument_grad (grad_X, grad_Y, width, height);
	
		
		int i,j,offset;
		
		double[] result = new double[MAX_VOTE]; 		
		
		int vote, votant=0;
		for(i=0;i<MAX_VOTE;i++)
			result[i]=0;
		for(j=1;j<height-1;j++) {
			offset=j*width+1;
			for(i=1;i<width-1;i++,offset++) {
				
				/*computing the directions*/
				double a = ((angle[offset]+Math.PI)*MAX_VOTE/2/Math.PI)+0.5;
				
				if ( ( (int)(a - 0.000001) ) != ( (int) (a + 0.000001) ) ) {
					a += 0.000001;
				}
				
				vote = ( (int)(a) ) % MAX_VOTE;
								
				votant++;
				
				result[vote]+=norm[offset]/(Math.sqrt(255*255+255*255));
			}
		}
		
		/*writing debugging images*/
		/*if (debugHoG) {
			Util.writeDoubletoPGM (grad_X, width, height, -256.0, +256.0, "Ix");
			Util.writeDoubletoPGM (grad_Y, width, height, -256.0, +256.0, "Iy");
			Util.writeDoubletoPGM (norm, width, height, 0, +363.0, "norm");
			Util.writeDoubletoPGM (angle, width, height, "theta");
		}*/
		
		/*normalizing*/
		for(i=0;i<MAX_VOTE;i++) {
			//System.out.println("MAX_VOTE : " + i + " , " + result[i] + " , " + votant);
			result[i]/=votant;
		}
		return result;
	}
	
	/*END HOG JONATHAN*/
	
	/*HOG DALAL*/

	public double[] HoGFixedNumberOfCells2 (BufferedImage image, int numCellsX, int numCellsY, int blockSize)
	{
		boolean debugHoG = false;
		
		double scale = 1.0;

		boolean size_ok = false;

		/*Image width and height*/
		int width = -1, height = -1;

		/*Image pixels*/
		int n = -1;

		/*Image color channels*/
		int channels = 3;

		/* Number of bins. Nine bins stands for the interval [0-180] degrees*/
		int numBins = 9;

		/*Cell size in pixels in X direction*/
		int cellSizeX = -1;

		/*Cell size in pixels in  direction*/
		int cellSizeY = -1;

		BufferedImage I = null;

		while (!size_ok) {
			
			I = Util.imageScale (image, scale);

			/*Image width*/
			width  = I.getWidth();

			height = I.getHeight(null);

			/*Image pixels*/
			n = width * height;

			/*Cell size in pixels in X direction*/
			cellSizeX = (int)(Math.floor((double)width/(double)numCellsX));

			/*Cell size in pixels in  direction*/
			cellSizeY = (int)(Math.floor((double)height/(double)numCellsY));

			if ( (cellSizeX >= 1) || (cellSizeY >= 1) ) {
				size_ok = true;
				//System.err.println("Ok!\n");
			}
			else {
				System.err.println("Error, cell size isn't enought, resizing!\n");
				scale++;
			}
		}
		/*Number of Cells*/
		int numCells = numCellsX * numCellsY;

		/*Squared block size in cells, for example 1x1, 2x2, 4x4 cells, ...*/
		if ( (blockSize > numCellsX) || (blockSize > numCellsY) || (blockSize <= 0) ) {
			System.err.println("Error!\n");
			return null;
		}

		/*Block overlap in cells*/
		int blockOverlap = 2; 
				
		if (numCells == 1) {
		   blockOverlap = 1;
		}
		
		if ( ((blockOverlap % blockSize) != 0) ) {
			System.err.println("Error!\n");
			return null;
		}

		/*Image derivative in X*/
		double[] Ix = new double[channels*n];

		/*Image derivative in Y*/
		double[] Iy = new double[channels*n];

		double[] Itheta = new double[n];

		double[] Inorm = new double[n];

		int[] Iarray = Util.getColorImageChannels (I);

		double[] Tnorm = new double[channels];

		for (int h = 1; h < (height - 1); h++) {

			for (int w = 1; w < (width - 1); w++) {

				int tempPos = h * width + w;

				for (int c = 0; c < channels; c++)
				{
					int x1 = getPosPixel ((h+0), (w-1), c, channels, channels*width);
					int x2 = getPosPixel ((h+0), (w+1), c, channels, channels*width);
					int y1 = getPosPixel ((h-1), (w+0), c, channels, channels*width);
					int y2 = getPosPixel ((h+1), (w+0), c, channels, channels*width);
					int p  = getPosPixel ((h+0), (w+0), c, channels, channels*width);

					/*Getting image gradients*/
					Ix[p] = -Iarray[x1] + Iarray[x2];
					Iy[p] = -Iarray[y1] + Iarray[y2];

					/*Gradient magnitude |G| = sqrt(Ix^2 + Iy^2)*/
					Tnorm[c] = Math.sqrt( Ix[p] * Ix[p] + Iy[p] * Iy[p] );
				}

				/*Find max norm, ...*/
				double maxNorm = 0;

				int maxNormIndex = 0;

				for (int c = 0; c < channels; c++)
				{
					if (Tnorm[c] > maxNorm)
					{
						maxNorm = Tnorm[c];
						maxNormIndex = c;

					}
				}
				/*Get the maximum norm of all channels*/
				Inorm[tempPos] = maxNorm;

				int normArrayIndex = channels*(tempPos) + maxNormIndex;

				if (Ix[normArrayIndex] == 0) {
					Ix[normArrayIndex] = .0000001; /*change to smallest non-zero positive number*/
				}

				/*Gradient orientation: \teta = arctan(Iy/Ix) */
				Itheta[tempPos] = Math.atan(Iy[normArrayIndex]/Ix[normArrayIndex]);
			}
		}

		/*Creating a matrix to hold the cell histogram*/
		double[][] cellHistogram = new double[numCells][numBins];
		double[][] normHistogram = new double[numCells][numBins];

		for (int cell = 0; cell < numCells; cell++) {
			for (int b = 0; b < numBins; b++) {
				cellHistogram[cell][b] = 0;
				normHistogram[cell][b] = 0;
			}
		}

		int cell = 0, pixels = 0;

		for (int cellY = 0; cellY < numCellsY; cellY++) {

			for (int cellX = 0; cellX < numCellsX; cellX++) {

				int h = cellY * cellSizeY;

				int w = cellX * cellSizeX;

				for (int yCellIter = 0; yCellIter < cellSizeY; yCellIter++) {

					for (int xCellIter = 0; xCellIter < cellSizeX; xCellIter++) {

						int pos = (h + yCellIter) * width + w + xCellIter;
						/* 180.0/Math.PI * .3491 = 20 degree = 9 bins in [0-180]*/
						int orientationBin = (int)Math.floor((Itheta[pos] + 1.5708)/.3491);

						if ( (orientationBin > (numBins-1)) || (orientationBin < 0) ) {
							System.err.println("Error: wrong orientation : " + orientationBin);
							return null;
						}

						/*Sum the gradient magnitudes of all pixels inside a cell with the same orientation*/
						cellHistogram[cell][orientationBin] += Inorm[pos];

						pixels++;
					}
				}
				cell++;
			}
		}

		if ( (cell != (numCells)) ){
			System.err.println("Error : gradient direction!\n");
			return null;
		}

		/*Getting the blockstep taking in account the blocks overlap*/
		int blockstep = blockSize - (blockOverlap / blockSize);
		
		if (blockstep == 0) {
			blockstep = 1;
		}

		/*Make shure that all blocks are totally inside the image*/
		int border = blockSize;

		int numBlocksX = 0, numBlocksY = 0;

		for (int blockY = 0; blockY <= (numCellsY - border); blockY += blockstep ) { numBlocksY++; }

		for (int blockX = 0; blockX <= (numCellsX - border); blockX += blockstep ) { numBlocksX++; }

		/* Hog descriptor: for each block, for each cell inside the block, we 
		 * construct and descriptor getting all directions of these cells*/
		double[] HoGDescriptor = new double[numBlocksX*numBlocksY*blockSize*blockSize*numBins];

		for (int ind = 0; ind < (numBlocksX*numBlocksY*blockSize*blockSize*numBins); ind++) {
			HoGDescriptor[ind] = -Double.MAX_VALUE;
		}
		
		int hogind = 0;

		/*Groups the cells in blocks and perform the normalization*/
		for (int blockY = 0; blockY <= (numCellsY - border); blockY += blockstep ) {

			for (int blockX = 0; blockX <= (numCellsX - border); blockX += blockstep ) {

				/*getting the indices of the central cell inside the block*/
				int indX = blockX + blockSize/2;

				int indY = blockY + blockSize/2;

				double sumOfSquares = 0;

				/*centralizing the block*/
				for (int cellY = -blockSize/2; cellY < (int)((blockSize+1)/2); cellY++) {

					for (int cellX = -blockSize/2; cellX < (int)((blockSize+1)/2); cellX++) {

						for (int bin = 0; bin < numBins; bin++) {

							int ind = (indY + cellY)*numCellsX + indX + cellX;
							if ( (ind >= 0) && (ind < numCells)) {
								double tempVal = cellHistogram [ind][bin];
								sumOfSquares += tempVal * tempVal;
							}
						}
					}
				}
				/*Normalizing by L2-norm:*/
				double blockNorm = Math.sqrt(sumOfSquares + 0.0000001);

				for (int cellY = -blockSize/2; cellY < (int)((blockSize+1)/2); cellY++) {

					for (int cellX = -blockSize/2; cellX < (int)((blockSize+1)/2); cellX++) {

						for (int bin = 0; bin < numBins; bin++) {

							int ind = (indY + cellY)*numCellsX + indX + cellX;
							if ( (ind >= 0) && (ind < numCells)) {
								double tempVal = cellHistogram [ind][bin];
								normHistogram [ind][bin] = tempVal/blockNorm;
								HoGDescriptor[hogind] = tempVal/blockNorm;
							}
							hogind++;
						}
					}
				}
			}
		}

		/*Ensures that all positions of the descriptors were appropriately computed*/
		for (int ind = 0; ind < (numBlocksX*numBlocksY*blockSize*blockSize*numBins); ind++) {
			if (HoGDescriptor[ind] == -Double.MAX_VALUE) {
				System.err.println("Error : position unitialized in the hog descriptor!\n");
				return null;
			}
		}
		/*writing debugging images*/
		if (debugHoG) {

			Util.writeDoubletoPGM (Inorm, width, height, 0, 363.0,"norm");
			Util.writeDoubletoPGM (Itheta, width, height, 0, Math.PI,"theta");
			Util.writeDoubletoPGM (Ix, channels*width, height, -256.0, 256.0,"Ix");
			Util.writeDoubletoPGM (Iy, channels*width, height, -256.0, 256.0, "Iy");

			int XhistSize = (int) (numCellsX*Math.sqrt(numBins));
			int YhistSize = (int) (numCellsY*Math.sqrt(numBins));

			double[] arrayA = new double[XhistSize*YhistSize];
			double[] arrayB = new double[XhistSize*YhistSize];

			for (int cellY = 0; cellY < YhistSize; cellY++)
			{
				for (int cellX = 0; cellX < XhistSize; cellX++)
				{
					int p = cellY * XhistSize + cellX;
					int celPos = (int)( cellY/Math.sqrt(numBins) )*numCellsX + (int)( cellX/Math.sqrt(numBins) ) ;
					int binPos = (int)((cellY % Math.sqrt(numBins)) * Math.sqrt(numBins) + (cellX % Math.sqrt(numBins)) );
					arrayA[p] = (int)(100*cellHistogram[celPos][binPos]);
					arrayB[p] = (int)(1000*normHistogram[celPos][binPos]);
				}
			}
			Util.writeDoubletoPGM (arrayA, XhistSize, YhistSize, 0, 363.0, "cellHist");
			Util.writeDoubletoPGM (arrayB, XhistSize, YhistSize, 0, 1.0, "normHist");

		}
		return HoGDescriptor;
	}

            
	
	/*blocksize: Squared block size in cells, for example 1x1, 2x2, 4x4 cells, ...*/
	public double[] HoGFixedNumberOfCells (BufferedImage I, int numCellsX, int numCellsY, int blockSize)
	{
		
		boolean debugHoG = false;

		/*Image width*/
		int width  = I.getWidth(null);

		/*Image height*/
		int height = I.getHeight(null);

		/*Image pixels*/
		int n = width * height;

		/*Image color channels*/
		int channels = 3; 

		/* Number of bins. Nine bins stands for the interval [0-180] degrees*/
		int numBins = 9; 

		/*Cell size in pixels in X direction*/
		int cellSizeX = (int)(Math.floor((double)width/(double)numCellsX));

		/*Cell size in pixels in  direction*/
		int cellSizeY = (int)(Math.floor((double)height/(double)numCellsY));

		if ( (cellSizeX < 1) || (cellSizeY < 1) ) {
			System.err.println("Error, cell size isn't enought!\n");
			return null;
		}
		
		/*Number of Cells*/
		int numCells = numCellsX * numCellsY;

				
		if ( (blockSize > numCellsX) || (blockSize > numCellsY) || (blockSize <= 0) ) {
			System.err.println("Error!\n");
			return null;
		}

		/*Block overlap in cells*/
		int blockOverlap = 2; 
				
		if (numCells == 1) {
		   blockOverlap = 1;
		}
		
		if ( ((blockOverlap % blockSize) != 0) ) {
			System.err.println("Error!\n");
			return null;
		}

		/*Image derivative in X*/
		double[] Ix = new double[channels*n];

		/*Image derivative in Y*/
		double[] Iy = new double[channels*n];

		double[] Itheta = new double[n]; 

		double[] Inorm = new double[n]; 

		int[] Iarray = Util.getColorImageChannels (I);

		double[] Tnorm = new double[channels];

		for (int h = 1; h < (height - 1); h++) {

			for (int w = 1; w < (width - 1); w++) {

				int tempPos = h * width + w;

				for (int c = 0; c < channels; c++)
				{
					int x1 = getPosPixel ((h+0), (w-1), c, channels, channels*width);
					int x2 = getPosPixel ((h+0), (w+1), c, channels, channels*width);
					int y1 = getPosPixel ((h-1), (w+0), c, channels, channels*width);
					int y2 = getPosPixel ((h+1), (w+0), c, channels, channels*width);
					int p  = getPosPixel ((h+0), (w+0), c, channels, channels*width);

					/*Getting image gradients*/
					Ix[p] = -Iarray[x1] + Iarray[x2];
					Iy[p] = -Iarray[y1] + Iarray[y2];
					
					/*Gradient magnitude |G| = sqrt(Ix^2 + Iy^2)*/
					Tnorm[c] = Math.sqrt( Ix[p] * Ix[p] + Iy[p] * Iy[p] );
				}

				/*Find max norm, ...*/
				double maxNorm = 0;

				int maxNormIndex = 0;

				for (int c = 0; c < channels; c++)
				{
					if (Tnorm[c] > maxNorm)
					{
						maxNorm = Tnorm[c];
						maxNormIndex = c;
					}
				}
				/*Get the maximum norm of all channels*/
				Inorm[tempPos] = maxNorm;

				int normArrayIndex = channels*(tempPos) + maxNormIndex;

				if (Ix[normArrayIndex] == 0) {
					Ix[normArrayIndex] = .0000001; /*change to smallest non-zero positive number*/
				}

				/*Gradient orientation: \teta = arctan(Iy/Ix) */
				Itheta[tempPos] = Math.atan(Iy[normArrayIndex]/Ix[normArrayIndex]);
			}
		}
		
		/*Creating a matrix to hold the cell histogram*/
		double[][] cellHistogram = new double[numCells][numBins];
		double[][] normHistogram = new double[numCells][numBins];
		
		for (int cell = 0; cell < numCells; cell++) {
			for (int b = 0; b < numBins; b++) {
				cellHistogram[cell][b] = 0;
				normHistogram[cell][b] = 0;
			}
		}		

		int cell = 0, pixels = 0;
		
		for (int cellY = 0; cellY < numCellsY; cellY++) {

            for (int cellX = 0; cellX < numCellsX; cellX++) {

                int h = cellY * cellSizeY;

                int w = cellX * cellSizeX;

				for (int yCellIter = 0; yCellIter < cellSizeY; yCellIter++) {

					for (int xCellIter = 0; xCellIter < cellSizeX; xCellIter++) {

						int pos = (h + yCellIter) * width + w + xCellIter;

						/* 180.0/Math.PI * .3491 = 20 degree = 9 bins in [0-180]*/
						int orientationBin = (int)Math.floor((Itheta[pos] + 1.5708)/.3491);
 	
						if ( (orientationBin > (numBins-1)) || (orientationBin < 0) ) {
							System.err.println("Error: wrong orientation : " + orientationBin);
							return null;
						}

						/*Sum the gradient magnitudes of all pixels inside a cell with the same orientation*/
						cellHistogram[cell][orientationBin] += Inorm[pos];

						pixels++;
					}
				}
				cell++;
			}
		}

		if ( (cell != (numCells)) ){
			System.err.println("Error : gradient direction!\n");
			return null;
		}

		/*Getting the blockstep taking in account the blocks overlap*/
		int blockstep = blockSize - (blockOverlap / blockSize);
		
		if (blockstep == 0) {
			blockstep = 1;
		}
		
		/*We ensures that all blocks are totally inside the image*/
		int border = blockSize;

		int numBlocksX = 0, numBlocksY = 0;

		for (int blockY = 0; blockY <= (numCellsY - border); blockY += blockstep ) { numBlocksY++; }
		
		for (int blockX = 0; blockX <= (numCellsX - border); blockX += blockstep ) { numBlocksX++; }

		/* Hog descriptor: for each block, for each cell inside the block, we 
		 * construct and descriptor getting all directions of these cells*/
		double[] HoGDescriptor = new double[numBlocksX*numBlocksY*blockSize*blockSize*numBins]; 
		
		for (int ind = 0; ind < (numBlocksX*numBlocksY*blockSize*blockSize*numBins); ind++) {
			HoGDescriptor[ind] = -Double.MAX_VALUE;
		}
		
		int hogind = 0;
		
		/*Groups the cells in block and perform the normalization*/
		for (int blockY = 0; blockY <= (numCellsY - border); blockY += blockstep ) {
			
			for (int blockX = 0; blockX <= (numCellsX - border); blockX += blockstep ) {

				/*getting the indices of the central cell inside the block*/
				int indX = blockX + blockSize/2;

				int indY = blockY + blockSize/2;

				double sumOfSquares = 0;
				
				/*centralizing the block*/
				for (int cellY = -blockSize/2; cellY < (int)((blockSize+1)/2); cellY++) {

					for (int cellX = -blockSize/2; cellX < (int)((blockSize+1)/2); cellX++) {

						for (int bin = 0; bin < numBins; bin++) {

							int ind = (indY + cellY)*numCellsX + indX + cellX;
							if ( (ind >= 0) && (ind < numCells)) {
								double tempVal = cellHistogram [ind][bin];
								sumOfSquares += tempVal * tempVal;
							}
						}
					}
				}
				
				/*Normalizing by L2-norm:*/
				double blockNorm = Math.sqrt(sumOfSquares + 0.0000001);

				for (int cellY = -blockSize/2; cellY < (int)((blockSize+1)/2); cellY++) {

					for (int cellX = -blockSize/2; cellX < (int)((blockSize+1)/2); cellX++) {

						for (int bin = 0; bin < numBins; bin++) {

							int ind = (indY + cellY)*numCellsX + indX + cellX;
							if ( (ind >= 0) && (ind < numCells)) {
								double tempVal = cellHistogram [ind][bin];
								normHistogram [ind][bin] = tempVal/blockNorm;
							    HoGDescriptor[hogind] = tempVal/blockNorm;
	  					    }
							hogind++;
						}
					}
				}
			}
		}
		
		/*Ensures that all positions of the descriptors were appropriately computed*/
		for (int ind = 0; ind < (numBlocksX*numBlocksY*blockSize*blockSize*numBins); ind++) {
			if (HoGDescriptor[ind] == -Double.MAX_VALUE) {
					System.err.println("Error : position unitialized in the hog descriptor!\n");
					return null;
			}
		}
		
        /*writing debugging images*/
		if (debugHoG) {
			
			Util.writeDoubletoPGM (Inorm, width, height, 0, 256.0, "norm");
			Util.writeDoubletoPGM (Itheta, width, height, 0, 256.0, "theta");
			Util.writeDoubletoPGM (Ix, channels*width, height, 0, 256.0, "Ix");
			Util.writeDoubletoPGM (Iy, channels*width, height, 0, 256.0, "Iy");
			
			int XhistSize = (int) (numCellsX*Math.sqrt(numBins));
			int YhistSize = (int) (numCellsY*Math.sqrt(numBins));
			
			double[] arrayA = new double[XhistSize*YhistSize];
			double[] arrayB = new double[XhistSize*YhistSize];
			
			for (int cellY = 0; cellY < YhistSize; cellY++)
			{
				for (int cellX = 0; cellX < XhistSize; cellX++)
				{
					int p = cellY * XhistSize + cellX;
					int celPos = (int)( cellY/Math.sqrt(numBins) )*numCellsX + (int)( cellX/Math.sqrt(numBins) ) ;
					int binPos = (int)((cellY % Math.sqrt(numBins)) * Math.sqrt(numBins) + (cellX % Math.sqrt(numBins)) );
	 				arrayA[p] = (int)(100*cellHistogram[celPos][binPos]);
	 				arrayB[p] = (int)(1000*normHistogram[celPos][binPos]);
				}
			}
			Util.writeDoubletoPGM (arrayA, XhistSize, YhistSize, 0, 256.0, "cellHist");
			Util.writeDoubletoPGM (arrayB, XhistSize, YhistSize, 0, 256.0, "normHist");
			
		}
		return HoGDescriptor;
	}
	
	/*END HOG DALAL*/
	
	public double[] hog (BufferedImage original) {
		
		int number_of_cells_x = 1;
		
		int number_of_cells_y = 3; //Changer ici 
		
		boolean debug = false;
		
		int new_height = 16;
		
		BufferedImage resize = Util.imageResize (original, new_height);
		
		/*Resized image width*/
		int width  = resize.getWidth();

		/*Resized image height*/
		int height = resize.getHeight();
		
		assert(height == new_height);
		
		/*Image pixels*/
		int rwt = 3;  
		
		int n = width * height;
		
		double[] grey = new double[n];
		
		double[] grel = new double[n];
		
		double[] dx = new double[n];
		
		double[] dy = new double[n];
		
		double[] dnorm = new double[n];
		
		double[] dtheta = new double[n];
		
		double[] weight = new double[2*rwt+1];
		
		compute_weights (weight, rwt);
		
		/*for (int i = 0; i < 2*rwt+1; i++) {
			System.err.printf(" %8.6f ", weight[i]);
		}
		System.err.printf("\n");*/
		
		get_grey_image (resize, grey);
		
		normalize_grey_image (grey, width, height, weight, rwt, grel);

		/*Number of Cells*/
		int number_of_cells = number_of_cells_x * number_of_cells_y;
		
		/*Number of gradient directions*/
		int bins_per_cell = 12;	// Original -> 8, 16 bem bom
				
		int total_bins = number_of_cells*bins_per_cell;
				
		/*Creating a matrix to hold the cell histogram*/
		double[] cells_histogram = new double[total_bins];
		
		double eps = 0.02; /*nominal deviation of pixel noise*/
		
		for (int i = 0; i < total_bins; i++) {
			cells_histogram[i] = 0;
		}
		
		for (int x = 1; x < (width - 1); x++) {

			for (int y = 1; y < (height - 1); y++) {

				int position = y * width + x;
				int kxm = position - 1;
				int kxp = position + 1;
				int kym = position - width;
				int kyp = position + width;
				
				dx[position] = (grel[kxp] - grel[kxm])/2;
				
				dy[position] = (grel[kyp] - grel[kym])/2;
				
				/*Compute the gradient norm but return zero if too small*/
				double d2 = dx[position]*dx[position] + dy[position]*dy[position];
				
				if (d2 <= eps*eps) {
					dnorm[position] = 0.0;
				}
				else {
					dnorm[position] = Math.sqrt(d2 - eps*eps);
				}
				
				dtheta[position] = Math.atan2(dy[position], dx[position]);
				
				if (dtheta[position] < 0) {
					dtheta[position] += Math.PI;
				}
				
				int cx = (number_of_cells_x * (x-1))/(width-1);
				
				assert( (cx >= 0) && (cx < number_of_cells_x)); 
				
				int cy = (number_of_cells_y * (y-1))/(height-1);
				
				assert( (cy >= 0) && (cy < number_of_cells_y));
				
				int c_pos = cy * number_of_cells_x + cx;
								
				int bin = (int) Math.floor(bins_per_cell*(dtheta[position]/Math.PI+1)+0.5);
				bin = bin % bins_per_cell;
				
				assert ( (bin >= 0) && (bin < bins_per_cell));
								
				int bin_pos = c_pos * bins_per_cell + bin;
				
				cells_histogram[bin_pos] += dnorm[position];
				
				
			}	
				
		}
			
		/*Normalize the histogram of each cell to unit L2 norm*/
		for (int cy = 0; cy < number_of_cells_y; cy++) {

			for (int cx = 0; cx < number_of_cells_x; cx++) {

				int c_pos = cy * number_of_cells_x + cx;

				double sum_v2 = 0.0;

				for (int bin = 0; bin < bins_per_cell; bin++) {
					int bin_pos = c_pos * bins_per_cell + bin;
					double v = cells_histogram [bin_pos];
					sum_v2 += v * v;
				}

				double cell_norm = Math.sqrt(sum_v2 + 0.0000001);

				for (int bin = 0; bin < bins_per_cell; bin++) {

					int bin_pos = c_pos * bins_per_cell + bin;
					double v = cells_histogram [bin_pos];
					cells_histogram [bin_pos] = v/cell_norm;
					if (debug) {
						System.err.printf("cell : %d,%d bin : %d, sum : %f\n", cx, cy, bin, cells_histogram [bin_pos]); 
					}
				}
			}	
		}
			
		
		if (debug) {
			
			Util.writeDoubletoPGM (grel, width, height, 0.0, 1.0, "norm");
			Util.writeDoubletoPGM (dx, width, height, -1.0, 1.0, "dx");
			Util.writeDoubletoPGM (dy, width, height, -1.0, 1.0, "dy");
			Util.writeDoubletoPGM (dnorm, width, height, 0.0, 1.42, "dnorm");
			Util.writeDoubletoPGM (dtheta, width, height, 0, Math.PI, "dtheta");

			try {
				String outname1 = "original.png";
				String outname2 = "resized.png";
				ImageIO.write(original, "png", new File(outname1));
				ImageIO.write(resize, "png", new File(outname2));
			}
			catch (Exception e1) {
				System.err.println("error: fail to save the images");
			}
		}	
		
		return cells_histogram;
	}
	
	
	/*AUXILIARY FUNCTIONS*/

	public void get_grey_image (BufferedImage image, double[] grey) {

		int w = image.getWidth(null);
		int h = image.getHeight(null);
		for (int y = 0; y < h; y++) {
			for (int x = 0; x < w; x++) {				
				int pixel = image.getRGB(x, y);
				int position = y * w + x;				
				double R = (pixel >> 16) & 255;
				double G = (pixel >> 8) & 255;
				double B = (pixel & 255);
				grey[position] = 0.299*R + 0.587*G + 0.114*B;
			}
		}
	}
	
	public void compute_weights (double[] weight, int rwt) {
		int nwt = 2*rwt+1;
		weight[0] = 1;
		for (int i = 1; i < nwt; i++) { 
			weight[i] = 0.0; 
			for (int j = i; j >=1; j--) {
				weight[j] = (weight[j] + weight[j-1])/2;
			}
			weight[0] /= 2;
		}
	}
	
	public void normalize_grey_image (double[] grey, int w, int h, double[] weight, int rwt, double[] grel) {
		double AVG, DEV;
		for (int y = 0; y < h; y++) {
			for (int x = 0; x < w; x++) {				
				int position = y * w + x;	
				AVG = get_grey_avg (grey, w, h, x, y, weight, rwt);
				DEV = get_grey_dev (grey, w, h, x, y, weight, rwt, AVG);
				grel[position] = (grey[position] - AVG)/DEV;
			}
		}
	
	}
	
	public double get_grey_avg (double[] grey, int w, int h, int x, int y, double[] weight, int rwt) {
		double sum_vwt = 0.0, sum_wt = 0.0;
		for (int dy = -rwt; dy <= rwt; dy++) {
			for (int dx = -rwt; dx <= rwt; dx++) {
				int x1 = x + dx;
				int y1 = y + dy;
				if ( (x1 >= 0) && (x1 < w) && (y1 >= 0) && (y1 < h)) {
				    int position = y1 * w + x1; 
				    double v = grey[position];
				    double wt = weight[rwt+dx]*weight[rwt+dx];
				    sum_vwt += v * wt;
				    sum_wt += wt; 
				}

			}
		}
		return sum_vwt/sum_wt;
	}
	
	public double get_grey_dev (double[] grey, int w, int h, int x, int y, double[] weight, int rwt, double AVG) {
		double sum_v2wt = 0.0, sum_wt = 0.0;
		for (int dy = -rwt; dy <= rwt; dy++) {
			for (int dx = -rwt; dx <= rwt; dx++) {
				int x1 = x + dx;
				int y1 = y + dy;
				if ( (x1 >= 0) && (x1 < w) && (y1 >= 0) && (y1 < h)) {
				    int position = y1 * w + x1; 
				    double v = grey[position]-AVG;
				    double wt = weight[rwt+dx]*weight[rwt+dx];
				    sum_v2wt += v * v * wt;
				    sum_wt += wt; 
				}

			}
		}
		double noise = 0.01; /*Assumed standard deviation of noise*/
		return Math.sqrt(sum_v2wt/sum_wt + noise*noise);
	}
	
	
	private int getPosPixel (int h, int w, int c, int nch, int ncols) {
		return (h * ncols + w*nch + c);
	}
}
