package regrouping;

import java.awt.image.BufferedImage;

import preprocessing.Util;

public class HOG {
	
	public final static int MAX_VOTE = 10;

	/*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, "Ix");
			Util.writeDoubletoPGM (grad_Y, width, height, "Iy");
			Util.writeDoubletoPGM (norm, width, height, "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[] HoGFixedNumberOfCells (BufferedImage I, int numCellsX, int numCellsY)
	{
		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;

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

		/*Block overlap in cells*/
		int blockOverlap = 2; 
		
		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);

		/*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, "norm");
			Util.writeDoubletoPGM (Itheta, width, height, "theta");
			Util.writeDoubletoPGM (Ix, channels*width, height, "Ix");
			Util.writeDoubletoPGM (Iy, channels*width, height, "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, "cellHist");
			Util.writeDoubletoPGM (arrayB, XhistSize, YhistSize, "normHist");
			
		}
		return HoGDescriptor;
	}
	
	/*END HOG DALAL*/
	
	
	/*AUXILIARY FUNCTIONS*/
	
	private int getPosPixel (int h, int w, int c, int nch, int ncols) {
		return (h * ncols + w*nch + c);
	}
		

	public double[] HOG_func (BufferedImage I, boolean centered, int binNum, int numCellsX, int numCellsY, int blockSize, String scheme, double epsilon ) {

	        //Matrix RGBimg[ 3 ]; /* array of 2D matrices, each being the RGB components of img */
	        /* Hor: horizontal, Ver: vertical, Mag: magnitude, Bin: orientation bin */
	        //Matrix gradHor[ 3 ], gradVer[ 3 ], gradMag, gradBin;
	        //Matrix tempMag, tempVote, tempBin, tempBlock;
		
		    int M = I.getHeight(), N = I.getWidth();
		    
		    /*Cell size in pixels in X direction*/
		    int cellSizeX = (int)(Math.floor((double)N/(double)numCellsX));

		    /*Cell size in pixels in  direction*/
		    int cellSizeY = (int)(Math.floor((double)M/(double)numCellsY));
           
		    int j, k, row, col, Hcount, Vcount, HH, VV, cRow, cCol, bRow, bCol;
	        int BCratio = blockSize / cellSizeX; /* the "r" in comment [D] */
	        int BCratioSquare = BCratio * BCratio;
	        /* number of cells in vertical and horizontal directions:*/
	        int cellV = M / cellSizeY, cellH = N / cellSizeX;
	        /* number of blocks in horizontal and vertical directions */
	        int blockV = cellV - BCratio + 1; /* overlapping blocks */
	        int blockH = cellH - BCratio + 1; /* overlapping blocks */
	        int blockNum = blockV * blockH;
	        
	      
	        
	        /*Image color channels*/
			int channels = 3; 
	  
			/*Image pixels*/
			int n = N * M;
			
	        /*Image derivative in X*/
			double[] Ix = new double[channels*n];

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

			double[][] Itheta = new double[M][N]; 

			double[][] Inorm = new double[M][N]; 
			
			int[][] gradBin = new int[M][N]; 
						
			int[] Iarray = Util.getColorImageChannels (I);

			double[] Tnorm = new double[channels];
	        
			zeros(Itheta, N, M);
		    
			zeros(Inorm, N, M);			
			
	        for (int h = 1; h < (M - 1); h++) {

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

					int tempPos = h * N + w;

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

						/*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[h][w] = maxNorm;

					int normArrayIndex = channels*(tempPos) + maxNormIndex;
					
					if (Ix[normArrayIndex] == 0) {
						Ix[normArrayIndex] = .0000001; /*change to smallest non-zero positive number*/
					}
					
					//if ( Inorm[h][w] > gradMag[row][col] ) {
						double ang;
						int bin, binSize = 360 / binNum;
						/* compute the angle and adjust for II, III, IV quadrants */
						ang = Math.atan( Iy[normArrayIndex]/Ix[normArrayIndex] ) / Math.PI * 180.0; /* arc tangent */
						if ( Ix[normArrayIndex] < 0 ) { /* II, III */
							ang += 180.f;
						}
						else if ( Iy[normArrayIndex] < 0 ) { /* IV */
							ang += 360.f;
						}
						assert( ang >= 0 && ang < 360.f );
						/* convert angle into orientation bin */
						/* 2009.07.28: 避免角度落在兩個bin之間的boundary effects，不該直接轉成bin，
						 * 而要用SIFT Sec.6.1說的"trilinear interpolation"把票分配給鄰近的兩個bin。
						 */
						bin = (int)ang / binSize;
						assert( bin >= 0 && bin < binNum );
						gradBin[h][w] = bin;
					//}
				}
			}
	        
	        /* generate Gaussian filter for HOG */
	        double Gauss[][] = new double[cellSizeY][cellSizeX];
	        Gaussian (Gauss, cellSizeX, cellSizeY, 8.0);
	        
	        double[][] tempBlock = new double[BCratioSquare][binNum]; 
	       
	        double[][] tempMag = new double[cellSizeY][cellSizeX]; 
	        
	        double[][] tempVote = new double[cellSizeY][cellSizeX]; 

			int[][] tempBin = new int[cellSizeY][cellSizeX]; 
	        
			zeros(tempMag, cellSizeX, cellSizeY);
			
			zeros(tempVote, cellSizeX, cellSizeY);
		    
			zeros(tempBin, cellSizeX, cellSizeY);
			
			zeros(tempBlock, binNum, BCratioSquare);

			int i = 0; // cell count (for debugging)
		       
			int size_hog = 0;
			for ( row = 0, Vcount = 0; Vcount < blockV; row += cellSizeY, Vcount++ ) {
				for ( col = 0, Hcount = 0; Hcount < blockH; col += cellSizeX, Hcount++ ) {
					for ( VV = 0; VV < BCratio; VV++ ) {
						for ( HH = 0; HH < BCratio; HH++ ) {
							size_hog++;
						}
					}
				}
			}
			
			double[] HoGDescriptor = new double[size_hog*BCratioSquare*binNum]; 
			
			for (int ind = 0; ind < (size_hog*BCratioSquare*binNum); ind++) {
				HoGDescriptor[ind] = -Double.MAX_VALUE;
			}
			
			int hog_index = 0;
	        for ( row = 0, Vcount = 0; Vcount < blockV; row += cellSizeY, Vcount++ ) {
	        	for ( col = 0, Hcount = 0; Hcount < blockH; col += cellSizeX, Hcount++ ) {
	        		/* for each (overlapping) block of BCratio x BCratio cell array: */
	        		j = 0; // cell count within a block
	        		for ( VV = 0; VV < BCratio; VV++ ) {
	        			for ( HH = 0; HH < BCratio; HH++ ) {
	        				/* for each cell in the block: */
	        				int rowBeg = row + VV * cellSizeY;
	        				int rowEnd = rowBeg + cellSizeY - 1;
	        				int colBeg = col + HH * cellSizeX;
	        				int colEnd = colBeg + cellSizeX - 1;
	        				
                            /* extract a cell from gradMag, gradBin */
                            part_assign( Inorm, rowBeg, rowEnd, colBeg, colEnd, tempMag, 0, cellSizeX - 1, 0, cellSizeY - 1);
                            part_assign( gradBin, rowBeg, rowEnd, colBeg, colEnd, tempBin, 0, cellSizeX - 1, 0, cellSizeY - 1);
                            
                            e_mul (tempMag, cellSizeX, cellSizeY, Gauss, cellSizeX, cellSizeY, tempVote, cellSizeX, cellSizeY);
                            
                            for ( cRow = 0; cRow < cellSizeY; cRow++ ) {
                            	for ( cCol = 0; cCol < cellSizeX; cCol++ ) {
                            		/* for each pixel in the cell */
                            		int bin = tempBin[cRow][cCol];
                            		tempBlock[j][bin] += tempVote[cRow][cCol];
                            	}
                            }

                            j++; i++;

                            double norm = v_norm2 (tempBlock, binNum, BCratioSquare);
                            s_mul (tempBlock, 1.f / Math.sqrt( norm * norm + epsilon * epsilon ), binNum, BCratioSquare);
                            
                            /*** [4] Collect HOG's over detection window ***/
                            for (bRow = 0; bRow < BCratioSquare; bRow++ ) {
                            	for (bCol = 0; bCol < binNum; bCol++ ) {
                            		HoGDescriptor[hog_index] = tempBlock[bRow][bCol];
                            		hog_index++;
                            	}
                            }

                            /* end of each block: */
                            /* clear tempBlock (restart from 0) */
                            zeros(tempBlock, binNum, BCratioSquare);
                                     
                    	}
	        		}
	        	}
	        }

	        /*Ensures that all positions of the descriptors were appropriately computed*/
			for (int ind = 0; ind < (size_hog*BCratioSquare*binNum); ind++) {
				if (HoGDescriptor[ind] == -Double.MAX_VALUE) {
						System.err.println("Error : position unitialized in the hog descriptor!\n");
						System.exit(1);
				}
			}
	        
			return HoGDescriptor;
	}
	
	void e_mul 
		(	double[][] m1, int m1_w, int m1_h,
			double[][] m2, int m2_w, int m2_h,
			double[][] m3, int m3_w, int m3_h
		) 
	{
		int row, col;
		for ( row = 0; row < m1_h; row++ ) {
			/* multiplication */
			for ( col = 0; col < m1_w; col++ ) {
				m3[row][col] = m1[row][col] * m2[row][col];
			}
		}

	}
	
	void part_assign ( 
			double source[][], int sRowBegin, int sRowEnd, int sColBegin, int sColEnd, 
			double target[][], int dRowBegin, int dRowEnd, int dColBegin, int dColEnd ) {

		int sRow, sCol, dRow, dCol;
		/* check dimensions */
		/*if ( sRowEnd - sRowBegin != dRowEnd - dRowBegin || sColEnd - sColBegin != dColEnd - dColBegin ) {
			System.err.println( "part_assign(): Dimensions disagree." );
			System.exit(1);
		}*/

		for ( sRow = sRowBegin, dRow = dRowBegin; sRow <= sRowEnd; sRow++, dRow++ ) {
			for ( sCol = sColBegin, dCol = dColBegin; sCol <= sColEnd; sCol++, dCol++ ) {
				target[ dRow ][ dCol ] = source[ sRow ][ sCol ];
			}
		}

	}

	void part_assign ( 
			int source[][], int sRowBegin, int sRowEnd, int sColBegin, int sColEnd, 
			int target[][], int dRowBegin, int dRowEnd, int dColBegin, int dColEnd ) {

		int sRow, sCol, dRow, dCol;
		/* check dimensions */
		/*if ( sRowEnd - sRowBegin != dRowEnd - dRowBegin || sColEnd - sColBegin != dColEnd - dColBegin ) {
			System.err.println( "part_assign(): Dimensions disagree." );
			System.exit(1);
		}*/

		for ( sRow = sRowBegin, dRow = dRowBegin; sRow <= sRowEnd; sRow++, dRow++ ) {
			for ( sCol = sColBegin, dCol = dColBegin; sCol <= sColEnd; sCol++, dCol++ ) {
				target[ dRow ][ dCol ] = source[ sRow ][ sCol ];
			}
		}

	}
	
	
	
	
	
	
	
	/* 2D gradient filters: horizontal/vertical; centered/uncentered */
	void gradient(double[][][] source, int source_width, int source_height, double[][][] dest, int dsize, String dir, boolean centered ) {
        int row, col;
	        
	    zeros (dest, source_width, source_height, 1);

	        /* 分成四種case各別處理 */
	        if ( dir.equals("horizontal") && !centered ) {
	                /* [-1.,1] */
	                for ( row = 0; row < source_height; row++ ) {
	                        for ( col = 0; col < source_width - 1; col++ ) {
	                                dest[ 0 ][ row ][ col ] = source[ 0 ][ row ][ col + 1 ] - source[ 0 ][ row ][ col ];
	                        }
	                        /* boundary: rightmost */
	                        dest[ 0 ][ row ][ col ] = source[ 0 ][ row ][ col - 1 ] - source[ 0 ][ row ][ col ];
	                }
	        }
	        else if ( dir.equals("horizontal") && centered ) {
	                /* [-1,0,1] */
	                for ( row = 0; row < source_height; row++ ) {
	                        /* boundary: leftmost */
	                        dest[ 0 ][ row ][ 0 ] = source[ 0 ][ row ][ 1 ] - source[ 0 ][ row ][ 0 ];
	                        for ( col = 1; col < source_width - 1; col++ ) {
	                                dest[ 0 ][ row ][ col ] = source[ 0 ][ row ][ col + 1 ] - source[ 0 ][ row ][ col - 1 ];
	                        }
	                        /* boundary: rightmost */
	                        dest[ 0 ][ row ][ col ] = source[ 0 ][ row ][ col ] - source[ 0 ][ row ][ col - 1 ];
	                }

	        }
	        else if ( dir.equals("vertical") && !centered ) {
	                /* [-1.,1]T */
	                for ( row = 0; row < source_height - 1; row++ ) {
	                        for ( col = 0; col < source_width; col++ ) {
	                                dest[ 0 ][ row ][ col ] = source[ 0 ][ row + 1 ][ col ] - source[ 0 ][ row ][ col ];
	                        }
	                }
	                /* boundary: bottom */
	                for ( col = 0; col < source_width; col++ ) {
	                        dest[ 0 ][ row ][ col ] = source[ 0 ][ row - 1 ][ col ] - source[ 0 ][ row ][ col ];
	                }
	        }
	        else if ( dir.equals("vertical") && centered ) {
	                /* [-1,0,1]T */
	                /* boundary: top */
	                for ( col = 0; col < source_width; col++ ) {
	                        dest[ 0 ][ 0 ][ col ] = source[ 0 ][ 1 ][ col ] - source[ 0 ][ 0 ][ col ];
	                }
	                for ( row = 1; row < source_height - 1; row++ ) {
	                        for ( col = 0; col < source_width; col++ ) {
	                                dest[ 0 ][ row ][ col ] = source[ 0 ][ row + 1 ][ col ] - source[ 0 ][ row - 1 ][ col ];
	                        }
	                }
	                /* boundary: bottom */
	                for ( col = 0; col < source_width; col++ ) {
	                        dest[ 0 ][ row ][ col ] = source[ 0 ][ row ][ col ] - source[ 0 ][ row - 1 ][ col ];
	                }
	        }
	        else {
	             System.err.printf( "gradient(): unexpected gradient type." );
	             System.exit(1);
	        }
	}

	void zeros (double[][] matrix, int width, int height) {
		int row, col;
		for ( row = 0; row < height; row++ ) {
			for ( col = 0; col < width; col++ ) {
				matrix[ row ][ col ] = 0.0;
			}
		}
	}
	
	void zeros (int[][] matrix, int width, int height) {
		int row, col;
		for ( row = 0; row < height; row++ ) {
			for ( col = 0; col < width; col++ ) {
				matrix[ row ][ col ] = 0;
			}
		}
	}
	
	void zeros(double[][][] source, int width, int height, int nchannels) {
		int row, col, layer;
		for ( layer = 0; layer < nchannels; layer++ ) {
			for ( row = 0; row < height; row++ ) {
				for ( col = 0; col < width; col++ ) {
					source[ layer ][ row ][ col ] = 0.f;
				}
			}
		}
	}

	
	void Gaussian(double[][] dest, int sizeX, int sizeY, double sigma ) {
	    /* 2D Gaussian filter with 0 mean and sigma stddev:
	     *                      1                x^2 + y^2
	     * Gauss( x, y ) = ------------ * exp( - --------- )
	     *                 2*pi*sigma^2          2*sigma^2
	     *
	         * Need to adjust to make the sum = 1
	     */
	         int x, y;
	         float center = ( sizeX - 1 ) / 2.f;
	         double sum;
	         zeros(dest, sizeX, sizeY);

	         for ( x = 0; x < sizeY; x++ ) {
	                 float xVal = x - center;
	                 for ( y = 0; y < sizeX; y++ ) {
	                        float yVal = y - center;
	                        /* skip the denominator, because we'll normalize the matrix, anyway. */
	                        dest[ x ][ y ] = Math.exp( -( xVal*xVal + yVal*yVal ) / ( 2.f*sigma*sigma ) );
	                 }
	         }
	         /* adjust to make the sum = 1 */
	         sum = m_sum (dest, sizeX, sizeY);
	         s_mul( dest, 1.0 / sum, sizeX, sizeY);
	}

	void s_mul(double[][] source, double number, int width, int height) {
		int row, col;
		for ( row = 0; row < height; row++ ) {
			for ( col = 0; col < width; col++ ) {
				source[ row ][ col ] *= number;
			}
		}
      
	}

	double m_sum(double[][] source, int width, int height) {
		int row, col;
		double sum = 0.0;
		for ( row = 0; row < height; row++ ) {
			for ( col = 0; col < width; col++ ) {
				sum += source[row][col];
			}
		}
		return sum;
	}

	double v_norm2(double[][] source, int width, int height) {
		int row, col;
		double norm = 0.0;
		for ( row = 0; row < height; row++ ) {
			for ( col = 0; col < width; col++ ) {

				double value = source[ row ][ col ];
				norm += value * value;
			}
		}
		return Math.sqrt( norm );
	}

		
}
