package descriptors;
import java.awt.BasicStroke;
import java.awt.Color;
import java.awt.Graphics2D;
import java.awt.Polygon;
import java.awt.image.BufferedImage;
import java.io.File;
import java.io.IOException;
import javax.imageio.ImageIO;
import preprocessing.Lanczos;
import preprocessing.Util;

public class HOG {

	public double[] hog_descriptor (
			
			boolean debug,
			
			BufferedImage image, /*Original image*/
			
            int new_height,        
			
            int number_of_cells_x,
			int number_of_cells_y,
			int bins_per_cell,
			
			boolean image_normalization,
			String image_normalization_weight, 
			double image_normalization_weight_radius,
			
			String gradient_option,
			
			String histogram_normalization_metric,
			
			String weight_function,
			
			boolean deformable_weights ) {
	
		
		/*settings[0] = gradient image (0 - Dalal, 1 - Ours) */
		/*settings[1] = histogram binning (0 - Dalal, 1 - Ours) */
		/*settings[2] = use cell weight mask (for Dalal) for setting[1] true only */
		int[] settings = {1, 1, 1}; 
		
		/*Settings*/
		//int safe_margin = 1;		//-> Best (graficos) -> [0-30]
		int safe_margin = 0; /*pixels*/
		
		double noise = 0.02; /*Assumed standard deviation of noise*/ //-> Best Journal IEEE
		//double noise = 0.01; /*Assumed standard deviation of noise*/ //-> Resultados gerados com isso [0-30]
		//double noise = 0.005; //-> Best (graficos)
		
		double black_level = 0.02; /*Assumed black level of image*/
		
		boolean image_logscale = false;
		
		int number_of_cells = number_of_cells_x * number_of_cells_y; /*HOG cells*/

		int number_of_bins = number_of_cells * bins_per_cell; /*HOG bins*/
		
		/*Image resizing. If the new height is negative the region is not resized.*/
		BufferedImage image_resized = null;	 	

		if (new_height > 0) {
			/*Resizing by Lanczos interpolation.*/
			Lanczos lanczos = new Lanczos();
			image_resized = lanczos.lanczos(image, new_height, 1);
            assert(image_resized.getHeight() == new_height);
		}
		else {
			image_resized = Util.imageScale (image, 1.0);
		}
		/*End-resizing*/

		/*Getting the resized dimensions.*/
		int rwidth = image_resized.getWidth();
		
		int rheight = image_resized.getHeight();		
        
		/*Verifying if the image was resized to {new_height)}.*/
		assert((rheight == new_height) || (new_height ==  -1));

		/*Number of pixels of the resized image.*/
		int n = rwidth * rheight;

		/*Normalized image*/
		double[] grel = new double[n];

		/*Image norm.*/
		double[] dnorm = new double[n];

		/**/
		double[] dtheta = new double[n];
		
		/*Creating a matrix to hold the cells histogram.*/
		double[] cells_histogram = new double[number_of_bins];
		for (int i = 0; i < number_of_bins; i++) {
			cells_histogram[i] = 0;
		}
		if ( (rwidth <= 2) || (rheight <= 2) ) {
			return cells_histogram;
		}
		
		/*Computing weights {x_weight, y_weight} to normalize the resized image.*/
		
		int x_weight_rad = -1, y_weight_rad = -1;
		
		double x_weight[] = null, y_weight[] = null;
		
		if (image_normalization_weight.compareTo("Gauss") == 0) {		
			/*Gaussian weights*/
			x_weight_rad = choose_gaussian_weight_size (image_normalization_weight_radius * rheight);		
			y_weight_rad = choose_gaussian_weight_size (1.0 * rheight);	
			
			x_weight = new double [2 * x_weight_rad + 1];
			y_weight = new double [2 * y_weight_rad + 1];
			
			compute_gaussian_weights (x_weight, x_weight_rad);		
			compute_gaussian_weights (y_weight, y_weight_rad);
		}
		else if (image_normalization_weight.compareTo("Binomial") == 0) { 
			/*Binomial weights*/
			x_weight_rad = y_weight_rad = (int) image_normalization_weight_radius;
			
			x_weight = new double [2 * x_weight_rad + 1];
			y_weight = new double [2 * y_weight_rad + 1];
			
			compute_binomial_weights (x_weight, x_weight_rad);
			compute_binomial_weights (y_weight, y_weight_rad);
		}
		else {
			System.err.printf("Error: choose a valid weight to the image normalization\n");
			System.exit(1);
		}

		/*Convert to grey scale*/
		double[] grey = get_grey_image (image_resized);		
		if (debug){
			/*Writing image norm*/  
			Util.writeDoubletoPGM (grey, rwidth, rheight, 0.0, 1.0, "grey");
		}
		
		/*Image normalization*/
		if (image_normalization) {			 
			if ( (x_weight != null) &&  (y_weight != null) ) {
				grel = normalize_grey_image (grey, rwidth, rheight, x_weight, x_weight_rad, y_weight, y_weight_rad, noise);
			}
			else {
				System.err.printf("Error: choose a valid image_normalization_weight\n");
				System.exit(1);
			}
		}
		else {
			for (int i = 0; i < n; i++) { grel[i] = grey[i]; }

		}		
		if (debug){
			Util.writeDoubletoPGM (grel, rwidth, rheight, 0.0, 1.0, "norm");
		}
		
		/*Convert to log scale*/
		if (image_logscale) {
			convert_to_log_scale(grel, rwidth, rheight, black_level);
		}
		
		if (debug){
			Util.writeDoubletoPGM (grel, rwidth, rheight, 0.0, 1.0, "glog");
		}
			
		/*Computing the approximate baselines {ytop, ybot} to each column.*/
		
		double[][] baselines = null;
		
		if (deformable_weights) {
			/*Computing the variable text-baselines {ytop, ybot} using image normalization.*/
			//int tmp_rwt = choose_gaussian_weight_size (0.75 * rheight); -> Best
			int tmp_rwt = choose_gaussian_weight_size (1.0 * rheight);
			double[] dist_weight = new double[2 * tmp_rwt + 1];
			compute_gaussian_weights (dist_weight, tmp_rwt);
			baselines = text_baselines (rwidth, rheight, dist_weight, tmp_rwt, grel);
		}
		else {
			/*Without variable text-baselines weights*/
			baselines = new double[rwidth][2];
			for (int w = 0; w < rwidth; w++) {
				//baselines[w][0] = rheight;// - safe_margin - 1; /*ybot*/
				//baselines[w][1] = 0;//safe_margin; /*ytop*/ 
				baselines[w][0] = rheight - safe_margin - 1; /*ybot*/
				baselines[w][1] = safe_margin; /*ytop*/ 
			}	
		}

		/*Creating a matrix to hold the cells weights.*/
		double[][] cell_weight_image = new double[number_of_cells][n];
		
		if (settings[0] == 0) {
			get_mag_theta_dalal (image_resized, dnorm, dtheta, rwidth, rheight, debug); 	
		}
		else {
			if (gradient_option.compareTo("Sobel") == 0) {
				safe_margin = 1;
			}
			get_mag_theta (grel, dnorm, dtheta, rwidth, rheight, safe_margin, noise, gradient_option, debug);
		}
		
		float[][][] hist = new float[number_of_cells_x][number_of_cells_y][bins_per_cell];
		for (int indx = 0; indx < number_of_cells_x; indx++) {
			for (int indy = 0; indy < number_of_cells_y; indy++) {
				for (int k = 0; k < bins_per_cell; k++) {
					hist[indx][indy][k] = (float)0.0;
				}
			}
		}
  		
		for (int x = safe_margin; x < (rwidth - safe_margin); x++) {

				for (int y = safe_margin; y < (rheight - safe_margin); y++) {

				int position = y * rwidth + x;
		
				/*Getting baselines.*/
				double xleft = safe_margin;
				
				double xright = rwidth - safe_margin;
				
				double ybot = baselines[x][0];
				
				double ytop = baselines[x][1];

				/*Computing the cells histogram and fuzzy weights*/
				for (int cx = 0; cx < number_of_cells_x; cx++) {

					double wtx = cell_weight (weight_function, number_of_cells_x, cx, x, xright, xleft);
					
					for (int cy = 0; cy < number_of_cells_y; cy++) {
															
						int c_pos = cy * number_of_cells_x + cx;
						
						double wty = cell_weight (weight_function, number_of_cells_y, cy, y, ybot, ytop);
						
						if (weight_function.compareTo("Dalal") == 0) {
							//wd = dalal_weight (x, y, number_of_cells_x, number_of_cells_y, rwidth, rheight);
							wty *= dalal_weight (x, y, 1, 1, rwidth, rheight);
						}
								
						int[] bin = new int[2];
						double[] factor = new double[2];
						
						//double wd = 1.0;
						if (settings[1] == 0) {
							//System.out.printf("%d %d ", x, y);
							get_bin_pos (hist, (float)(dnorm[position] * wtx * wty), x, y, number_of_cells_x, number_of_cells_y, rwidth, rheight, bins_per_cell, (float)(dtheta[position]), bin, factor );
							//System.out.printf(" %.14f %.14f %.14f\n", (float)(dnorm[position]*wd), wd, dnorm[position]);							
						}
						else if (settings[1] == 1) {
							get_bin_pos (bins_per_cell, dtheta[position], bin, factor);
						}
																	
						int bin_pos1 = c_pos * bins_per_cell + bin[0];
						
						int bin_pos2 = c_pos * bins_per_cell + bin[1];	
											
						cells_histogram[bin_pos1] += (dnorm[position] * wtx * wty) * factor[0];
						
						cells_histogram[bin_pos2] += (dnorm[position] * wtx * wty) * factor[1];
						
						//System.out.printf("bin_pos1 %d, bin_pos2 : %d, hist1 : %f, hist2 : %f, dnorm : %f, wd : %f, factor1 : %f factor2 : %f\n", bin_pos1, bin_pos2, cells_histogram[bin_pos1], cells_histogram[bin_pos2], dnorm[position], wtx * wty * wd, factor[0], factor[1]);							
												
						//System.out.printf("value : %f, weight %f, hist : %f\n", dnorm[position]*wd, dnorm[position]*wd*factor[0], cells_histogram[bin_pos1]);
						//System.out.printf("value : %f, weight %f, hist : %f\n", dnorm[position]*wd, dnorm[position]*wd*factor[1], cells_histogram[bin_pos2]);
						
						cell_weight_image[c_pos][position] = wtx * wty;
					}
				}			
			}
		}
		
		/*int index = 0;
		for (int indx = 0; indx < number_of_cells_x; indx++) {
            for (int indy = 0; indy < number_of_cells_y; indy++) {
                for (int k = 0; k < bins_per_cell; k++) {
                	cells_histogram[index] = hist[indx][indy][k];
                	index++;
                  	//System.out.printf(" %.14f ", hist[indx][indy][k]);
                }
            }
		}*/
					
		/*Normalize the histogram of each cell to unit L1 or L2 norm*/
		double sum = 0.0;
		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 = 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];
					
					if (histogram_normalization_metric.compareTo("L1") == 0) {
						sum += v; 
					}
					else if (histogram_normalization_metric.compareTo("D1") == 0) {
						sum += v; 
					}
					else if (histogram_normalization_metric.compareTo("D2") == 0) {
						sum += v; 
					}
					else if (histogram_normalization_metric.compareTo("L2") == 0) {
						sum += v * v;
					}
					else {
						System.err.printf("Error: choose a valid normalization option\n");
						System.exit(1);
					}
				}
			}
		}
		
		//System.err.printf("Sum : %.9f\n", sum);
			
			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 cell_norm = 0.0;
				
				if (histogram_normalization_metric.compareTo("L1") == 0) {
					cell_norm = sum + 1.0e-100; 
				}
				else if (histogram_normalization_metric.compareTo("D1") == 0) {
					//cell_norm = sum + 1.0*bins_per_cell;
					cell_norm = sum + 1.0*bins_per_cell*number_of_cells_x*number_of_cells_y;
				}
				else if (histogram_normalization_metric.compareTo("D2") == 0) {
					cell_norm = 1.0;					
				}
				else if (histogram_normalization_metric.compareTo("L2") == 0) {
					cell_norm = Math.sqrt(sum + 1.0e-100);
				}
				else {
					System.err.printf("Error: choose a valid normalization option\n");
					System.exit(1);
				}

				for (int bin = 0; bin < bins_per_cell; bin++) {
					
					int bin_pos = c_pos * bins_per_cell + bin;
					
					double v = cells_histogram [bin_pos];
					
					//System.err.printf("cell : %f\n", v);
										
					cells_histogram [bin_pos] = (float)(v/cell_norm);
				}
			}
		}
				
		if (debug) {
			
			try {
				/*Writing original image.*/
				ImageIO.write(image, "png", new File("original.png"));
				
				/*Writing resized image.*/
				ImageIO.write(image_resized, "png", new File("resized.png"));
				
			} catch (IOException e) {
				e.printStackTrace();
			}

			/*Writing baselines*/			
			double[] tmp = new double[n];

			/*Copying the resized and normalized grey-level image.*/
			for (int i = 0; i < n; i++) { tmp[i] = grel[i]; }

			for (int x = 1; x < (rwidth - 1); x++) {

				/*Getting the baselines extremes.*/
				int ytop = (int)(baselines[x][0] + 0.5) * rwidth + x;

				int ybot = (int)(baselines[x][1] + 0.5) * rwidth + x;

				/*Writing in black the baselines extremes.*/
				if ( (ytop > 0) && (ytop < (rwidth * rheight))) {
					tmp[ytop] = -2;
				}
				if ( (ybot > 0) && (ybot < (rwidth * rheight))) {
					tmp[ybot] = -2;
				}
			}
			Util.writeDoubletoPGM (tmp, rwidth, rheight, -2.0, 2.0, "image_baselines");			  
			/*End-baselines*/  

			/*Writing image gradient directions*/ 
			double dnorm_max = 0.5;

			Util.writeDoubletoPGM (dnorm, rwidth, rheight, 0.0, dnorm_max, "dnorm");

			Util.writePolartoPNG (dnorm, dtheta, rwidth, rheight, dnorm_max, 0.0, Math.PI, "dpolar");

			Util.writeDoubletoPGM (dtheta, rwidth, rheight, 0, Math.PI, "dtheta");
			
			/*Writing weight functions*/ 
			for (int nc = 0; nc < number_of_cells; nc++) {
				Util.writeDoubletoPGM (cell_weight_image[nc], rwidth, rheight, 0.0, 1.0, "cwt_"+ String.format("%02d", nc));
			}

			/*Writing HOG descriptors as circular images*/ 

			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[] histogram = new double[bins_per_cell];
					
					for (int bin = 0; bin < bins_per_cell; bin++) {
						
						int bin_pos = c_pos * bins_per_cell + bin;
						
						histogram[bin] = cells_histogram [bin_pos];
						
						//System.err.printf("cell : %d,%d bin : %d, sum : %.18f\n", cx, cy, bin, cells_histogram [bin_pos]);
					}
					
					/*To each cell print the HOG histogram.*/
					print_histogram (image_resized, histogram, number_of_cells_x, number_of_cells_y, bins_per_cell, histogram_normalization_metric, "hog_directions_" + cy + "_" + cx + ".png");
				}
			}			
		}
		return cells_histogram;
	} /*End-HOG*/	
	
	public void get_bin_pos (int bins_per_cell, double dtheta, int[] bin, double[] factor)
	{
		/*Computing the bin according the gradient direction.*/
		//boolean full_circ = false; //[0-30]
		
		boolean full_circ = true;
		//boolean full_circ = false;
		
		double period = (full_circ ? 2 * Math.PI : Math.PI);

		double a = bins_per_cell*(dtheta/period+1);
		int ia = (int)Math.floor(a);
		double frac = a - (double)ia;

		bin[0] = (ia + bins_per_cell) % bins_per_cell;
		bin[1] = (ia + 1) % bins_per_cell;
		factor[0] = (1-frac);
		factor[1] = frac;
		
		assert ( (bin[0] >= 0) && (bin[0] < bins_per_cell));
		assert ( (bin[1] >= 0) && (bin[1] < bins_per_cell));		
	}
	
	public void get_bin_pos (
			float hist[][][],
			float weight,
			int x, 
			int y,
			int number_of_cells_x,
			int number_of_cells_y,
			int rwidth, 
			int rheight, 
			int bins_per_cell, 
			float dtheta, 
			int[] bin, 
			double[] factor)
	{
		boolean full_circ = true;
		
		//double[] extent = { (double)rwidth/number_of_cells_x, (double)rheight/number_of_cells_y};
		
		int number_of_blocks_x = 1;
		
		int number_of_blocks_y = 1;
		
		double[] extent = { (double)rwidth/number_of_blocks_x, (double)rheight/number_of_blocks_y};
		
		float angle = (float) (dtheta * 180.0/Math.PI + 180.0);
		
		if (!full_circ) {
			angle %= 180;
		}
		
		//System.out.printf("theta : %f, angle : %f \n", dtheta, angle);
		//System.out.printf(" %d   ", (int)(Math.floor(angle)));
		
		System.out.printf("*********************\n");
		
		
		/*float[][][] H = new float[2][2][2]; 
		
        for (int indx = 0; indx < number_of_cells_x; indx++) {
                for (int indy = 0; indy < number_of_cells_y; indy++) {
                        for (int k = 0; k < bins_per_cell; k++) {
                                H[indx][indy][k] = (float)0.0;
                        }
                }
        }*/
						
		compute_bin (hist, weight, rwidth, rheight, (double)((x % extent[0]) + 0.5), (double)((y % extent[1])+0.5), (int)(Math.floor(angle)), number_of_cells_x, number_of_cells_y, bins_per_cell, full_circ);

		/*bin[0] = (int)r[0][1];
		bin[1] = (int)r[1][1];
		factor[0] = r[0][0];
		factor[1] = r[1][0];
		
		System.out.printf("-> %d %d %f %f\n", bin[0], bin[1], factor[0], factor[1]);*/
		
	}
	
	/***************************Gradient functions****************************/
	
	public void get_mag_theta (
			double[] grel, 
			double[] dnorm,
			double[] dtheta,
			int rwidth, 
			int rheight, 
			int safe_margin, 
			double noise,
			String gradient_option,
			boolean debug) 
	{
		int n = rwidth * rheight;
		
		/*Image derivative in x direction.*/
		double[] dx = new double[n];

		/*Image derivative in y direction.*/
		double[] dy = new double[n];	
		
		double eps = Math.sqrt(2.0)*noise; /*Assumed deviation of noise in gradient*/
		
		double eps2 = eps * eps;
		
		for (int x = safe_margin; x < (rwidth - safe_margin); x++) {

			for (int y = safe_margin; y < (rheight - safe_margin); y++) {

				int position = y * rwidth + x;

				/*Computing image gradients*/
				double[] grad = new double[2];

				if (gradient_option.compareTo("Sobel") == 0) {
					gradient_sobel (grel, rwidth, rheight, x, y, grad);
				}
				else if (gradient_option.compareTo("Simple") == 0) {
					gradient_simple (grel, rwidth, rheight, x, y, grad);
				}
				else  {
					System.err.printf("Error: choose a valid gradient option\n");
					System.exit(1);
				}

				dx[position] = grad[0];
				dy[position] = grad[1];

				/*Computing the gradient norm but return zero if too small*/
				double d2 = dx[position]*dx[position] + dy[position]*dy[position];

				if (d2 <= eps2) {
					dnorm[position] = 0.0;
				}
				else {
					dnorm[position] = Math.sqrt(d2 - eps2);
				}

				/*Computing the gradient direction.*/
				dtheta[position] = Math.atan2(dy[position], dx[position]);

				if (dtheta[position] < 0) {
					//dtheta[position] += Math.PI;
					dtheta[position] += 2*Math.PI;
				}	
			}
		}
		
		if (debug) {
			/*Writing image gradients*/  

			Util.writeDoubletoPGM (dx, rwidth, rheight, -1.0, 1.0, "dx");

			Util.writeDoubletoPGM (dy, rwidth, rheight, -1.0, 1.0, "dy");
		}
	}
	
	public void get_mag_theta_dalal (
			BufferedImage image,
			double[] dnorm,
			double[] dtheta,
			int rwidth, 
			int rheight,
			boolean debug) {
		
		int n = rwidth * rheight;
		
		/*Image derivative in x direction.*/
		double[] dxx = new double[n];

		/*Image derivative in y direction.*/
		double[] dyy = new double[n];
		
		int w = image.getWidth();
		int h = image.getHeight();
		
		assert (w == rwidth);
		
		assert (h == rheight);
		
		/*double[][][] rgb = new double[w][h][3];
		double[][][] mag = new double[w][h][3];
		double[][][] theta = new double[w][h][3];
		double[][][] dx = new double[w][h][3];
		double[][][] dy = new double[w][h][3];*/
		
		float[][][] rgb = new float[w][h][3];
		float[][][] mag = new float[w][h][3];
		float[][][] theta = new float[w][h][3];
		float[][][] dx = new float[w][h][3];
		float[][][] dy = new float[w][h][3];

		for (int x = 0; x < w; x++) {
			for (int y = 0; y < h; y++) {				
				int pixel = image.getRGB(x, y);
				rgb[x][y][0] = (pixel >> 16) & 255;
				rgb[x][y][1] = (pixel >> 8) & 255;
				rgb[x][y][2] = (pixel & 255);
				rgb[x][y][0] = (float) Math.sqrt(rgb[x][y][0]);
				rgb[x][y][1] = (float) Math.sqrt(rgb[x][y][1]);
				rgb[x][y][2] = (float) Math.sqrt(rgb[x][y][2]);
			}			
		}

		/*Dy*/
		for (int x = 0; x < w; x++) {
			for (int y = 1; y < h-1; y++) {				
				/*dy[x][y][0] = (rgb[x][y+1][0] - rgb[x][y-1][0])/2.0;
				dy[x][y][1] = (rgb[x][y+1][1] - rgb[x][y-1][1])/2.0;
				dy[x][y][2] = (rgb[x][y+1][2] - rgb[x][y-1][2])/2.0;*/
				dy[x][y][0] = (rgb[x][y+1][0] - rgb[x][y-1][0]);
				dy[x][y][1] = (rgb[x][y+1][1] - rgb[x][y-1][1]);
				dy[x][y][2] = (rgb[x][y+1][2] - rgb[x][y-1][2]);
			}
		}
		for (int x = 0; x < w; x++) {
			for (int k = 0; k < 3; k++) {
				dy[x][0][k] = dy[x][1][k];
				dy[x][h-1][k] = dy[x][h-2][k];
			}
		}
		/*End Dy*/

		/*Dx*/
		for (int x = 1; x < w-1; x++) {
			for (int y = 0; y < h; y++) {				
				/*dx[x][y][0] = (rgb[x+1][y][0] - rgb[x-1][y][0])/2.0;
				dx[x][y][1] = (rgb[x+1][y][1] - rgb[x-1][y][1])/2.0;
				dx[x][y][2] = (rgb[x+1][y][2] - rgb[x-1][y][2])/2.0;*/
				dx[x][y][0] = (rgb[x+1][y][0] - rgb[x-1][y][0]);
				dx[x][y][1] = (rgb[x+1][y][1] - rgb[x-1][y][1]);
				dx[x][y][2] = (rgb[x+1][y][2] - rgb[x-1][y][2]);
			}			
		}
		for (int y = 0; y < h; y++) {
			for (int k = 0; k < 3; k++) { 
				dx[0][y][k] = dx[1][y][k];
				dx[w-1][y][k] = dx[w-2][y][k];
			}
		}
		/*End Dx*/

		/*Mag*/
		for (int x = 0; x < w; x++) {
			for (int y = 0; y < h; y++) {
				for (int k = 0; k < 3; k++) {
					mag[x][y][k] = (float) Math.sqrt((dx[x][y][k]*dx[x][y][k]) + (dy[x][y][k]*dy[x][y][k]));
				}
			}			
		}
		/*End Mag*/

		/*Theta*/
		for (int x = 0; x < w; x++) {
			for (int y = 0; y < h; y++) {
				for (int k = 0; k < 3; k++) {
					theta[x][y][k] = (float) Math.atan2(dy[x][y][k],dx[x][y][k]);				    
				}
			}			
		}
		/*End Mag*/

		for (int x = 0; x < w; x++) {
			for (int y = 0; y < h; y++) {
				int position = y * rwidth + x;
				double imaxMag = -Double.MAX_VALUE;
				double imaxTheta = -Double.MAX_VALUE;
				for (int k = 0; k < 3; k++) {
					if (mag[x][y][k] > imaxMag) {
						imaxMag = mag[x][y][k];
						imaxTheta = theta[x][y][k];
						dxx[position] = dx[x][y][k];
						dyy[position] = dy[x][y][k];
					}
				}
				//System.out.printf("[%.8f %.8f], ", imaxMag, imaxTheta);
				dnorm[position] = imaxMag;
				dtheta[position] = imaxTheta;
			}			
		}	
		
		
		if (debug) {
			/*Writing image gradients*/  

			Util.writeDoubletoPGM (dxx, rwidth, rheight, -1.0, 1.0, "dx");

			Util.writeDoubletoPGM (dyy, rwidth, rheight, -1.0, 1.0, "dy");
		}	
		
	}
	
	/*Sobel*/
	public void gradient_sobel (double[] image, int width, int height, int x, int y, double[] grad) {
		
		int position = y * width + x;
		
		double vmo = image[position - 1];
		double vpo = image[position + 1];
		double vom = image[position - width];
		double vop = image[position + width];

		double vmm = image[position - 1 - width];
		double vmp = image[position - 1 + width];
		double vpm = image[position + 1 - width];
		double vpp = image[position + 1 + width];
		
		grad[0] = (vpm + 2*vpo + vpp - vmm - 2*vmo - vmp)/8.0;
		grad[1] = (vmm + 2*vom + vpm - vmp - 2*vop - vpp)/8.0;
	}
	
	/*Simple*/
	public void gradient_simple (double[] image, int width, int height, int x, int y, double[] grad) {

		int position_x = y * width + x;
		
		int position_y = y * width + x;
		
		if (x == 0) {
			position_x = y * width + (x + 1);
		}
		if (x == (width-1)) {
			position_x = y * width + (x - 1);
		}
		
		if (y == 0) {
			position_y = (y + 1) * width + x;
		}
		
		if (y == (height - 1)) {
			position_y = (y - 1) * width + x;
		}
		
		double kxm = image[position_x - 1];
		double kxp = image[position_x + 1];
		double kym = image[position_y - width];
		double kyp = image[position_y + width];

		grad[0] = (kxp - kxm)/2;
		grad[1] = (kyp - kym)/2;
		/*grad[0] = (kxp - kxm);
		grad[1] = (kyp - kym);*/
	}
	
	/****************************Weight functions*****************************/
	
	/* Computes the cell weight factor for one axis {z} (x or y). Given: number of cells 
	 * {ncz}, cell index {cz}, pixel index {z}, all along that axis. 
	 * The option selects the weight type. Adjusting the interval by {r_bot, r_top}.*/
	public double cell_weight (String weight_function, int ncz, int cz, int z, double zmax, double zmin) {
		
		if (ncz == 1) {
			return 1; 
		}
		
		double z_star = (z - zmin)/(zmax - zmin);

		if (weight_function.compareTo("Step") == 0) {
			return StepFunc (ncz,cz,z_star);
		}
		else if (weight_function.compareTo("Dalal") == 0) {
			//return dalal_weight2 (z, cz, (int)(zmax - zmin));
			return StepFunc (ncz,cz,z_star);			
		}
		else if (weight_function.compareTo("Bernstein") == 0){
			return Bernstein (ncz-1,cz,z_star);
		}
		else if (weight_function.compareTo("Core") == 0) {
			return EdgeCore (ncz,cz,z_star);
		}
		else if (weight_function.compareTo("Exp") == 0){
			return Exp (ncz,cz,z_star);
		}
		else {
			assert(false);
			System.exit(1);
			return 0;
		}
	}
		
	/*Divides the interval [0-1] into {n} equal parts and returns 1.0 if {z} 
	 *is in part number {k} (0..n-1), 0 otherwise. */
	public double StepFunc (int n, int k, double z) {
		assert((k >= 0) && (k < n));
		//if (z <= 0) { return (k == 0 ? 0 : 1); }
		//else if (z >= 1) { return (k == n ? 0 : 1); }
		if (z <= 0) { return (k == 0 ? 1 : 0); }
		else if (z >= 1) { return (k == (n - 1) ? 1 : 0); }
		else {
			return ( (k <= z*n) && (z*n < k+1) ? 1.0 : 0.0 );
		}
	}

	/*Computes the Bernstein polynomial of degree {n} and index {k} for the argument {z}.*/
	public double Bernstein (int n, int k, double z) {
		assert((k >= 0) && (k <= n));
		if (z <= 0) { return (k == 0 ? 1 : 0); }
		else if (z >= 1) { return (k == n ? 1 : 0); }
		else {
			double zmax = ((double)k)/((double)n);
			return BernsteinPoly(n,k,z)/BernsteinPoly(n,k,zmax);
		}
	}
	
	public double BernsteinPoly (int n, int k, double z) {
		assert((k >= 0) && (k <= n));
		double res = 1.0;
		for (int i = 0; i < k; i++) {
			res = (res * (n - i))/(i+1)*z;
		}
		return res*Math.pow(1-z,n-k);
	}

	/*An edge-core weight function. If {n == 1} returns 1, if (n == 2) returns 
	 *weight 1.0 near the edges, or 1.0 in the core region depending on {k}*/
	public double EdgeCore (int n, int k, double z) {
		assert(n == 2);
		assert((k >= 0) && (k < n));
		if ( (z <= 0) || (z >= 1) ) { return (k == 0 ? 1 : 0); }
		else {
			double v = 4 * z * (1 - z);
			v = v*v;
			return (k==0? 1 - v: v);
		}
	}
	
	/*An exponential weight function.*/	
	private double gaussian (double z, double mu, double sigma) {
		return Math.exp(- ((z - mu)*(z - mu))/(2 * sigma * sigma));
	}

	/*An exponential weight function. */
	/*public double Exp (int n, int k, double z) {

		double mu = 0.15;

		double sigma_A = 0.3;

		double sigma_B = 0.3;

		assert((n==1) || (n == 3));
		assert((k >= 0) && (k < n));

		if (k == 0) {
			return gaussian (z, -mu, sigma_A);
		}
		else if (k == 2) {
			return gaussian (z, 1+mu, sigma_A);
		}
		else if (k == 1) {
			return gaussian (z, 0.5, sigma_B);
		}
		else {
			assert(false);
			return 0;
		}
	}*/
	
	/*An exponential weight function. */
	/*public double Exp (int n, int k, double z) {

		double mu = 0.15;

		double sigma = 1.0;
		
		assert((k >= 0) && (k < n));

		double avg = -mu + (1 + 2 * mu)/(double)(n - 1)*k;
		
		double dev = sigma/n;
		
		return gaussian (z, avg, dev);		
	}*/
	
	/*Best*/
	/*public double Exp (int n, int k, double z) {

		double mu = 0.01;

		double sigma = 0.5; 
		
		assert((k >= 0) && (k < n));

		double avg = -mu + (1 + 2 * mu)/(double)(n - 1)*k;
		
		double dev = sigma/n;
		
		return gaussian (z, avg, dev);		
	}*/
	
	public double Exp (int n, int k, double z) {

		double mu = 0.01; //-> Best JOURNAL IEEE

		double sigma = 0.5; //-> Best JOURNAL IEEE
		
		assert((k >= 0) && (k < n));

		double avg = -mu + (1 + 2 * mu)/(double)(n - 1)*k;
		
		double dev = sigma/n;
		
		return gaussian (z, avg, dev);		
	}
		
	/**/
	public int choose_gaussian_weight_size (double dev) {
		double rwt = 2*dev;
		return (int)(Math.ceil(rwt));
	}
	
	/**/
	public void compute_binomial_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 compute_gaussian_weights (double[] weight, int rwt) {
		double eps = 0.01;
		double a = -Math.log(eps);
		int nwt = 2*rwt+1;
		
		for (int i = 0; i < nwt; i++) { 
			double z = (i - rwt)/((double)rwt);
			weight[i] = (1-z*z)*Math.exp(-a*z*z);
		}
	}
	
	/*************************Drawing HOG histograms**************************/
	
	/*Function used to draw the HOG descriptor with a circular shape, 
	 *where each bin is proportional to its gradient norm.*/
	public void print_histogram (BufferedImage image, double[] histogram, int number_of_cells_x, int number_of_cells_y, int bins_per_cell, String histogram_normalization_metric, String outname ) {
		
		/*HOG descriptor image dimensions*/		
		int width = 600;		
		int height = 600;

		/*Factor to magnify each HOG bin*/
		double magnify = -1;
		
		if (histogram_normalization_metric.compareTo("L1") == 0) {
			magnify = Math.sqrt(bins_per_cell) * 200;
		}
		else if (histogram_normalization_metric.compareTo("L2") == 0) {
			magnify = 1 * 200;
		}
		else if (histogram_normalization_metric.compareTo("D1") == 0) {
			magnify = Math.sqrt(bins_per_cell) * 200;
		}

		/*Circle radius*/
		double radius = 90;
		
		/*Circle center*/
		double c_x = width/2;		
		double c_y = height/2;
		
		int n = width * height;
				
		BufferedImage temp = new BufferedImage(width, height, BufferedImage.TYPE_INT_RGB);
		
		/*Defining the graphics settings.*/
		float[] hsbvals = new float[3];		
		Graphics2D g = temp.createGraphics();		
		g.setStroke(new BasicStroke(1.0f));			
		Color.RGBtoHSB(0, 0, 0, hsbvals);		
		g.setColor(Color.getHSBColor(hsbvals[0], hsbvals[1], hsbvals[2]));
		
		/*Background painting*/		
		int white = 0xFFFFFF;
		
		for (int i = 0; i < n; i++) {
			int x = i % width;
			int y = i / width;
			temp.setRGB(x, y, white);
		}
		
		/*Drawing HOG bins*/		
		for (int bin = 0; bin < bins_per_cell; bin++) {
		
			double theta_a = (bin+0.4)*Math.PI/bins_per_cell;
			
			double theta_b = (bin-0.4)*Math.PI/bins_per_cell;
	
			Polygon polygon = null;
			
			for (int k = 0; k < 2; k++) {
			
				polygon = draw_Polygon (c_x, c_y, radius, theta_a+k*Math.PI, theta_b+k*Math.PI, histogram[bin], magnify);
			
				g.fillPolygon(polygon);
			}			
		}
		g.dispose();
		
		try {
			ImageIO.write(temp, "png", new File(outname));
		}
		catch (Exception e) { 
			System.err.printf("cannot write image\n");
		}		
	} 
	
	
	/*Function to draw each HOG bin as a rectangle around a circle.*/
	/*Function used to draw each HOG bin as a rectangle around a circle.*/
	/*Assumes that the Y axis points up.*/
	private Polygon draw_Polygon (double c_x, double c_y, double radius, double theta_a, double theta_b, double value, double magnify) { 
		
		/*Taking each point of the rectangle*/
		double x1 = c_x + radius * Math.cos(theta_a);
		double y1 = c_y + radius * Math.sin(theta_a);
		
		double x2 = c_x + (radius + value*magnify) * Math.cos(theta_a);
		double y2 = c_y + (radius + value*magnify) * Math.sin(theta_a);
		
		double x3 = c_x + radius * Math.cos(theta_b);
		double y3 = c_y + radius * Math.sin(theta_b);
		
		double x4 = c_x + (radius + value*magnify) * Math.cos(theta_b);
		double y4 = c_y + (radius + value*magnify) * Math.sin(theta_b);
		
		/*Getting the polygon*/
		Polygon polygon = new Polygon();
		
		polygon.addPoint((int)x1, (int)y1);
		
		polygon.addPoint((int)x2, (int)y2);
		
		polygon.addPoint((int)x4, (int)y4);
		
		polygon.addPoint((int)x3, (int)y3); 
		
		return polygon;
	}

	/*********************** Text baselines functions ************************/
	
	public double[][] text_baselines (int w, int h, double[] weight, int rwt, double[] grel) {
		
		double[] s = new double[h];
		
		double[][] baselines = new double[w][2]; 
		
		for (int x = 0; x < w; x++) {

			for (int y = 0; y < h; y++) {	
				
				double sum_var = 0.0, sum_avg = 0.0, sum_w = 0.0;
				
				for (int dx = -rwt; dx <= rwt; dx++) {
					int x1 = x + dx;
					int position = y * w + x1;
					if ((x1 >= 0) && (x1 < w)) {
						sum_avg += weight[rwt+dx]*grel[position];
						sum_w += weight[rwt+dx]; 
					}
				}	
				assert(sum_w != 0.0); 
				
				double AVG = (sum_avg/sum_w);
				
				for (int dx = -rwt; dx <= rwt; dx++) {
					int x1 = x + dx;
					int position = y * w + x1;
					if ((x1 >= 0) && (x1 < w)) {
						sum_var += weight[rwt+dx]* ( (grel[position]-AVG)*(grel[position]-AVG) );
					}
				}				
				s[y] = (sum_var/sum_w);
			}
			double[] tmp = get_baselines_by_percentiles (h, s);
			baselines[x][0] = tmp[0];
			baselines[x][1] = tmp[1];
		}
		return baselines;
	}
	
	public double[] get_baselines_by_percentiles (int h, double[] s)		
	{
		//double percentile = 0.1; //-> Best
		double percentile = 0.2; //-> Best
		//double percentile = 0.15;
		
		double[] baselines = new double[2];

		double sum_s = 0.0;

		for (int y = 0; y < h; y++) {
			sum_s += s[y];
		}

		int top = 0;
		
		double sum_top = 0.0;
		
		while ( (top < h) && (sum_top+s[top] <= percentile*sum_s) ) {
			sum_top += s[top];
			top++;
		}
		
		if (top > 0) { top--; }
		
		int bot = h-1;
		
		double sum_bot = 0.0;
		
		while ( (bot >= 0) && (sum_bot+s[bot] <= percentile*sum_s) ) {
			sum_bot += s[bot];
			bot--;
		}
		
		if (bot < h-1) { bot++; }
		
		double y_med = 0.5*(top+bot);
		
		double y_delta_min = 0.2*h;
		
		double y_delta = Math.max(0.5*(bot-top), y_delta_min);

		baselines[0] = Math.min(y_med + y_delta,h-1); //ybot
		baselines[1] = Math.max(y_med - y_delta,0);   //ytop 

		return baselines;
	}
	
	public double[] get_baselines_by_med (int h, double[] s)		
	{
		double[] baselines = new double[2];

		double sum_s = 0.0;

		double sum_sy = 0.0;

		for (int y = 0; y < h; y++) {
			sum_sy += s[y] * y;
			sum_s += s[y];
		}

		double y_med = sum_sy/sum_s;

		double sum_sdy2 = 0.0;

		for (int y = 0; y < h; y++) {
			sum_sdy2 += s[y] * ( (y - y_med)*(y - y_med) );
			sum_s += s[y];
		}

		double y_dev = Math.sqrt(sum_sdy2/sum_s);
		
		double y_delta_min = 0.2*h;
		
		double y_delta = Math.max(3*y_dev, y_delta_min);

		baselines[0] = Math.min(y_med + y_delta,h); //ybot
		baselines[1] = Math.max(y_med - y_delta,0); //ytop 
		
		return baselines;
	}

	
	/************************* Auxiliary functions ***************************/

    /*Get a grey level image from a color image.*/
	public double[] get_grey_image (BufferedImage image) {

		
		int w = image.getWidth(null);
		
		int h = image.getHeight(null);
		
		double[] grey = new double[w*h];
		
		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); 
				
				R /= 255.0;
				G /= 255.0;
				B /= 255.0;
				/*R *= R;
				G *= G;
				B *= B;*/					
				grey[position] = 0.299*R + 0.587*G + 0.114*B;
				
			}
		}
		return grey;
	}
	
	public void convert_to_log_scale (double[] grey, int w, int h, double eps) {
		int n = w * h;
		for (int position = 0; position < n; position++) {
			grey[position] = (Math.log(grey[position] + eps) - Math.log(eps))/(Math.log(1+eps)-Math.log(eps));
		}
	}
	
	/*Get a normalized image from a grey level image.*/
	public double[] normalize_grey_image (double[] grey, int w, int h, double[] x_weight, int x_rwt, double[] y_weight, int y_rwt, double noise) {
		
		double[] grel = new double[w*h];
		
		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, x_weight, x_rwt, y_weight, y_rwt);
				DEV = get_grey_dev (grey, w, h, x, y, x_weight, x_rwt, y_weight, y_rwt, AVG, noise);
				grel[position] = (grey[position] - AVG)/(3*DEV)+0.5;
				//grel[position] = (grey[position] - AVG)/(4*DEV);
				//grel[position] = (grey[position] - AVG)/DEV+0.5; //-> Best do Best
				if (grel[position] < 0) { grel[position] = 0.0; }
				else if (grel[position] > 1) { grel[position] = 1.0; }
			}
		}
		return grel;
	}
	
	/*Get the averaged pixel value weighted by normalizing window*/
	public double get_grey_avg (double[] grey, int w, int h, int x, int y, double[] x_weight, int x_rwt, double[] y_weight, int y_rwt) {
		double sum_vwt = 0.0, sum_wt = 0.0;
		for (int dy = -y_rwt; dy <= y_rwt; dy++) {
			for (int dx = -x_rwt; dx <= x_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 = x_weight[x_rwt+dx]*y_weight[y_rwt+dy];
				    sum_vwt += v * wt;
				    sum_wt += wt; 
				}
			}
		}
		return sum_vwt/sum_wt;
	}
	 
	/*Get the deviation of a pixel given a normalizing window*/
	public double get_grey_dev (double[] grey, int w, int h, int x, int y, double[] x_weight, int x_rwt, double[] y_weight, int y_rwt, double AVG, double noise) {
		double sum_v2wt = 0.0, sum_wt = 0.0;
		for (int dy = -y_rwt; dy <= y_rwt; dy++) {
			for (int dx = -x_rwt; dx <= x_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 = x_weight[x_rwt+dx]*y_weight[y_rwt+dy];
				    sum_v2wt += v * v * wt;
				    sum_wt += wt; 
				}

			}
		}
		
		return Math.sqrt(sum_v2wt/sum_wt + noise*noise);
	}
	/*END*/
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	public void compute_bin (float hist[][][], float weight, int w, int h, double x, double y, int angle, int ncx, int ncy, int bins_per_cell, boolean full_circ) {

		int nbx = 1;
		int nby = 1;		
		
		int orientation = 0;
		
		if (full_circ) {
			orientation = 360; /* half circle */
		}
		else {
			orientation = 180; /* half circle */
		}
		int VDim = 3; /* Vector dimension */
		float[] vec = { (float)x, (float)y, (float) angle };
		// System.out.printf("vec : %f, %f, %f\n", vec[0], vec[1], vec[2]);
		float[] pI = toindex(vec, w, h, ncx, ncy, nbx, nby, bins_per_cell,
				orientation, VDim);
		
		//System.out.printf("pI : %.14f, %.14f, %.14f\n", pI[0], pI[1], pI[2]);

		int[] fI = { (int) Math.floor(pI[0]), (int) Math.floor(pI[1]),
				(int) Math.floor(pI[2]) };
		int[] cI = { (int) Math.ceil(pI[0]), (int) Math.ceil(pI[1]),
				(int) Math.ceil(pI[2]) };
		float[] d = { (float)(pI[0] - fI[0]), (float)(pI[1] - fI[1]),(float)(pI[2] - fI[2])};

		//System.out.printf("fI: %d, %d, %d\n", fI[0], fI[1], fI[2]);
		
		//System.out.printf("d: %.14f, %.14f, %.14f\n", d[0], d[1], d[2]);
		
		//int[] tvbin_ = { nbx, nby, bins_per_cell }; -> Por blocos
		
		int[] tvbin_ = { nbx, ncy, bins_per_cell };
		
		//System.out.printf("tvbin: %d, %d, %d\n", tvbin_[0], tvbin_[1], tvbin_[2]);
		System.out.printf("--------------------\n");
		boolean[] warp_ = { false, false, true };

		boolean[] lowervalid = { true, true, true };
		boolean[] uppervalid = { true, true, true };

		float onehalf_ = 1 / 2;

		for (int i = 0; i < VDim; ++i) {
			if (warp_[i]) {
				cI[i] %= tvbin_[i];
				fI[i] %= tvbin_[i];
				if (fI[i] < 0)
					fI[i] += tvbin_[i];
				if (cI[i] < 0)
					cI[i] += tvbin_[i];
			} else {
				if (cI[i] >= tvbin_[i] - onehalf_ || cI[i] < -onehalf_)
					uppervalid[i] = false;
				if (fI[i] < -onehalf_ || fI[i] >= tvbin_[i] - onehalf_)
					lowervalid[i] = false;
			}
		}
		interpol_linear(hist, weight, d, fI, cI, lowervalid, uppervalid);
	}
	
	
	/*public void compute_bin (float hist[][][], double weight, int w, int h, double x, double y, int angle, int ncx, int ncy, int bins_per_cell, boolean full_circ) {

		int nbx = 1;
		int nby = 1;		
		
		int orientation = 0;
		
		if (full_circ) {
			orientation = 360; 
		}
		else {
			orientation = 180; 
		}
		int VDim = 3; 
		double[] vec = { x, y, (double) angle };
		// System.out.printf("vec : %f, %f, %f\n", vec[0], vec[1], vec[2]);
		double[] pI = toindex(vec, w, h, ncx, ncy, nbx, nby, bins_per_cell,
				orientation, VDim);
		
		// System.out.printf("pI : %f, %f, %f\n", pI[0], pI[1], pI[2]);

		int[] fI = { (int) Math.floor(pI[0]), (int) Math.floor(pI[1]),
				(int) Math.floor(pI[2]) };
		int[] cI = { (int) Math.ceil(pI[0]), (int) Math.ceil(pI[1]),
				(int) Math.ceil(pI[2]) };
		double[] d = { pI[0] - fI[0], pI[1] - fI[1], pI[2] - fI[2] };

		//System.out.printf("fI: %d, %d, %d\n", fI[0], fI[1], fI[2]);
		
		//System.out.printf("d: %f, %f, %f\n", d[0], d[1], d[2]);
		
		//int[] tvbin_ = { nbx, nby, bins_per_cell }; -> Por blocos
		
		int[] tvbin_ = { nbx, ncy, bins_per_cell };
		
		//System.out.printf("tvbin: %d, %d, %d\n", tvbin_[0], tvbin_[1], tvbin_[2]);
		boolean[] warp_ = { false, false, true };

		boolean[] lowervalid = { true, true, true };
		boolean[] uppervalid = { true, true, true };

		double onehalf_ = 1 / 2;

		for (int i = 0; i < VDim; ++i) {
			if (warp_[i]) {
				cI[i] %= tvbin_[i];
				fI[i] %= tvbin_[i];
				if (fI[i] < 0)
					fI[i] += tvbin_[i];
				if (cI[i] < 0)
					cI[i] += tvbin_[i];
			} else {
				if (cI[i] >= tvbin_[i] - onehalf_ || cI[i] < -onehalf_)
					uppervalid[i] = false;
				if (fI[i] < -onehalf_ || fI[i] >= tvbin_[i] - onehalf_)
					lowervalid[i] = false;
			}
		}
		interpol_linear(hist, weight, d, fI, cI, lowervalid, uppervalid);
	} */
	
	/*float[] toindex (float[] value, int w, int h, int ncx, int ncy, int nbx, int nby, 
			int bins_per_cell, int orientation, int VDim) {
		float[] r = new float[VDim];
		float onehalf_ = (float) 0.5;
		//double[] half_bin = {0.5*nbx, 0.5*nby, 0.5*bins_per_cell}; -> Block
		float[] half_bin = {(float) (0.5*nbx), (float) (0.5*ncy), (float) (0.5*bins_per_cell)};
		float[] bandwidth = {(float)w/(float)ncx, (float)h/(float)ncy, (float)orientation/(float)bins_per_cell};
		//double[] cenBound = {(double)w/(double)ncx/2.0, (double)h/(double)ncy/2.0, (double)orientation/2.0};-> Block
		float[] cenBound = {(float)w/(float)ncx/(float)2.0, (float)h/(float)nby/(float)2.0, (float)orientation/(float)2.0};
		
		//int[] cenBound = {w/ncx/2, h/nby/2, orientation/2};
		
		
		System.out.printf("half_bin: %.14f, %.14f, %.14f\n", half_bin[0], half_bin[1], half_bin[2]);
		System.out.printf("v: %.14f, %.14f, %.14f\n", value[0], value[1], value[2]);
		System.out.printf("cenBound: %.14f, %.14f, %.14f\n", cenBound[0], cenBound[1], cenBound[2]);
		System.out.printf("bandwidth: %.14f, %.14f, %.14f\n", bandwidth[0], bandwidth[1], bandwidth[2]);
		System.out.printf("onehalf : %.14f\n", onehalf_);
		
		
		for (int i = 0; i < 3; i++) {
			r[i] = (half_bin[i] - onehalf_ + (float)(value[i] - cenBound[i])/(float)bandwidth[i]);
		}
    	return r;
	}*/
	
	float[] toindex (float[] value, int w, int h, int ncx, int ncy, int nbx, int nby, 
			int bins_per_cell, int orientation, int VDim) {
		float[] r = new float[VDim];
		double onehalf_ = 0.5;
		//double[] half_bin = {0.5*nbx, 0.5*nby, 0.5*bins_per_cell}; -> Block
		double[] half_bin = {0.5*nbx, 0.5*ncy, 0.5*bins_per_cell};
		double[] bandwidth = {(double)w/(double)ncx, (double)h/(double)ncy, (double)orientation/(double)bins_per_cell};
		//double[] cenBound = {(double)w/(double)ncx/2.0, (double)h/(double)ncy/2.0, (double)orientation/2.0};-> Block
		double[] cenBound = {(double)w/(double)ncx/2.0, (double)h/(double)nby/2.0, (double)orientation/2.0};

		/*System.out.printf("half_bin: %.14f, %.14f, %.14f\n", half_bin[0], half_bin[1], half_bin[2]);
		System.out.printf("v: %.14f, %.14f, %.14f\n", value[0], value[1], value[2]);
		System.out.printf("cenBound: %.14f, %.14f, %.14f\n", cenBound[0], cenBound[1], cenBound[2]);
		System.out.printf("bandwidth: %.14f, %.14f, %.14f\n", bandwidth[0], bandwidth[1], bandwidth[2]);
		System.out.printf("onehalf : %.14f\n", onehalf_);*/
		
		for (int i = 0; i < 3; i++) {
			r[i] = (float)(half_bin[i] - onehalf_ + (value[i] - (double)cenBound[i])/(double)bandwidth[i]);
		}
		return r;
	}
	
	
	/*double[] toindex (double[] value, int w, int h, int ncx, int ncy, int nbx, int nby, 
			int bins_per_cell, int orientation, int VDim) {
		double[] r = new double[VDim];
		double onehalf_ = 0.5;
		//double[] half_bin = {0.5*nbx, 0.5*nby, 0.5*bins_per_cell}; -> Block
		double[] half_bin = {0.5*nbx, 0.5*ncy, 0.5*bins_per_cell};
		double[] bandwidth = {(double)w/(double)ncx, (double)h/(double)ncy, (double)orientation/(double)bins_per_cell};
		//double[] cenBound = {(double)w/(double)ncx/2.0, (double)h/(double)ncy/2.0, (double)orientation/2.0};-> Block
		double[] cenBound = {(double)w/(double)ncx/2.0, (double)h/(double)nby/2.0, (double)orientation/2.0};
		
		for (int i = 0; i < 3; i++) {
			r[i] = half_bin[i] - onehalf_ + (value[i] - (double)cenBound[i])/(double)bandwidth[i];
		}
    	return r;
	}*/
	
	public void interpol_linear (float[][][] hist, float weight, float[] d, int[] lowerI, int[] upperI, boolean[] lowerValid, boolean[] upperValid) {

		//double[][] r = new double[2][2];
		
		//float[] c = {(float)(1-d[0]), (float)(1-d[1]), (float)(1-d[2])};
		
		double[] c = {(1-d[0]), (1-d[1]), (1-d[2])};
		
		//System.out.printf("c: %.14f, %.14f, %.14f\n", c[0], c[1], c[2]);

		//System.err.printf("%f, %f, %f, %f \n", d[0], d[1], d[2], weight);

		for (int i = 0; i < 2; ++i) {
			if ((i==0 && !lowerValid[0]) || (i != 0 && !upperValid[0]))
				continue;
			float iwt = (float)(i != 0 ? d[0] : c[0]);
			//System.out.printf("iwt : %.14f\n", iwt);
			int ipos = (i != 0 ? upperI[0] : lowerI[0]);

			for (int j = 0; j < 2; ++j) {
				if ((j==0 && !lowerValid[1]) || (j !=0 && !upperValid[1]))
					continue;
				
				double jwt = iwt*(j != 0 ? d[1] : c[1]);
				
				//System.out.printf("jwt : %.14f\n", jwt); 
				int jpos = (j != 0 ? upperI[1] : lowerI[1]);

				for (int k = 0; k < 2; ++k) {
					if ((k==0 && !lowerValid[2]) || (k != 0 && !upperValid[2]))
						continue;
					double kwt = (float)(jwt)*(k != 0 ? d[2] : c[2]);
					//double kwt = (jwt)*(k != 0 ? d[2] : c[2]);
					int kpos = (k != 0 ? upperI[2] : lowerI[2]);
					{
						//r[k][0] = kwt;
						//r[k][1] = kpos;
						hist[ipos][jpos][kpos] += (float)(weight*kwt);
						System.out.printf("ipos %d, jpos %d, kpos %d, value : %.14f, kwt %.14f, weight : %.14f, hist : %.14f\n", ipos, jpos, kpos, weight, kwt, weight*kwt, hist[ipos][jpos][kpos]);
					}
				}
			}
		}
		//return r;
	}
	
	/*public void interpol_linear (float[][][] hist, double weight, double[] d, int[] lowerI, int[] upperI, boolean[] lowerValid, boolean[] upperValid) {

		//double[][] r = new double[2][2];
		
		double[] c = {(1-d[0]), (1-d[1]), (1-d[2])};

		//System.err.printf("%f, %f, %f, %f \n", d[0], d[1], d[2], weight);

		for (int i = 0; i < 2; ++i) {
			if ((i==0 && !lowerValid[0]) || (i != 0 && !upperValid[0]))
				continue;
			double iwt = (i != 0 ? d[0] : c[0]);
			int ipos = (i != 0 ? upperI[0] : lowerI[0]);

			for (int j = 0; j < 2; ++j) {
				if ((j==0 && !lowerValid[1]) || (j !=0 && !upperValid[1]))
					continue;
				double jwt = iwt*(j != 0 ? d[1] : c[1]);
				int jpos = (j != 0 ? upperI[1] : lowerI[1]);

				for (int k = 0; k < 2; ++k) {
					if ((k==0 && !lowerValid[2]) || (k != 0 && !upperValid[2]))
						continue;
					double kwt = jwt*(k != 0 ? d[2] : c[2]);
					int kpos = (k != 0 ? upperI[2] : lowerI[2]);
					{
						//r[k][0] = kwt;
						//r[k][1] = kpos;
						hist[ipos][jpos][kpos] += weight*kwt;
						//System.out.printf("ipos %d, jpos %d, kpos %d, value : %f, kwt %f, weight : %f, hist : %f\n", ipos, jpos, kpos, weight, kwt, weight*kwt, hist[ipos][jpos][kpos]);
					}
				}
			}
		}
		//return r;
	}*/
	
    public double dalal_weight2 (int z, int ncz, int size) {
    	
    	assert((z >= 0) && (z <= size));
		if (z <= 0) { return (z == 0 ? 1 : 0); }
		else if (z >= 1) { return (z == size ? 1 : 0); }
		else {
		
		float wtscale = 1;
		
		int nb = 1;

		float cellsize = (float)size/(float)ncz;
		
		float extent = (float)size/ncz;
		
		float center = (float)extent/(float)2.0;
		
		float numcell = (float)nb;

		float var2 = (cellsize * numcell)/(2*wtscale);

		z %= extent;		
		float sum = (float)(z - center);
		sum *= sum / var2;
		return Math.exp(-sum);
		}
	}
	
	
	public float dalal_weight (int x, int y, int ncx, int ncy, int w, int h) {
		
		float wtscale = 1;
		
		int nbx = 1, nby = 1;

		float[] var2 = new float[2];

		float[] cellsize = { (float)w/(float)ncx, (float)h/(float)ncy};
		
		float[] extent = { (float)w/ncx, (float)h/ncy};
		
		float[] center = { (float)extent[0]/(float)2.0, (float)extent[1]/(float)2.0};
		
		float[] numcell = { (float)nbx, (float)nby};

		for (int i = 0; i < 2; i++) { 
			var2[i] = (cellsize[i] * numcell[i])/(2*wtscale);
			var2[i] *= var2[i]*2;
		}

		x %= extent[0];
		y %= extent[1];			
		
		float[] t = new float[2];
		for (int k = 0; k < 2; k++) {
			if (k == 0) {			    
				t[k] = x - center[k];
			}
			else {
				t[k] = y - center[k];
			}
			t[k] *= t[k] / var2[k];
		}
		float sum = (float)0.0;
		for (int k = 0; k < 2; k++) { sum += t[k]; }
		return (float)Math.exp(-sum);
	}
	
	/*public float dalal_weight (int x, int y, int ncx, int ncy, int w, int h) {
		
		double wtscale = 1;
		
		int nbx = 1, nby = 1;

		double[] var2 = new double[2];

		double[] cellsize = { (double)w/(double)ncx, (double)h/(double)ncy};
		
		double[] extent = { (double)w/ncx, (double)h/ncy};
		
		double[] center = { extent[0]/2.0, extent[1]/2.0};
		
		double[] numcell = { (double)nbx, (double)nby};

		for (int i = 0; i < 2; i++) { 
			var2[i] = (cellsize[i] * numcell[i])/(2*wtscale);
			var2[i] *= var2[i]*2;
		}

		x %= extent[0];
		y %= extent[1];			
		
		double[] t = new double[2];
		for (int k = 0; k < 2; k++) {
			if (k == 0) {			    
				t[k] = x - center[k];
			}
			else {
				t[k] = y - center[k];
			}
			t[k] *= t[k] / var2[k];
		}
		double sum = 0.0;
		for (int k = 0; k < 2; k++) { sum += t[k]; }
		return (float)Math.exp(-sum);
	}*/
}