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 javax.imageio.ImageIO;
import preprocessing.Lanczos;
import preprocessing.Util;

public class HOG {

	public double[] hog (
			BufferedImage original,
			double x_dwt,
			boolean gauss_norm_weight,
			int number_of_cells_x,
			int number_of_cells_y,
			int bins_per_cell,
			int new_height,
			boolean normalize_image,
			int option, /*0 - Nothing, 1 - Step, 2 - Bernstein*/
			boolean normalize_histogram_L1,
			boolean use_interval) {

		boolean debug = false;

		BufferedImage resize = null;	 	

		if (new_height > 0) {
			Lanczos lanczos = new Lanczos();
            resize = lanczos.lanczos(original, new_height);
            if (resize.getHeight() != new_height) {
            	resize = Util.imageResize (original, new_height);
            }
		}
		else {
			resize = Util.imageScale (original, 1.0);
		}

		/*Resized image width*/
		int width  = resize.getWidth();

		/*Resized image height*/
		int height = resize.getHeight();

		assert(height == new_height);

		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];
		
		int x_rwt, y_rwt;
		
		double x_weight[] = null, y_weight[] = null;
		if (gauss_norm_weight) {		
			x_rwt = choose_gaussian_weight_size(x_dwt*height);		
			y_rwt = choose_gaussian_weight_size(1.0*height);	
			x_weight = new double[2*x_rwt+1];
			y_weight = new double[2*y_rwt+1];
			compute_gaussian_weights (x_weight, x_rwt);		
			compute_gaussian_weights (y_weight, y_rwt);
		}
		else {
			x_rwt = y_rwt = (int) x_dwt;
			x_weight = new double[2*x_rwt+1];
			y_weight = new double[2*y_rwt+1];
			compute_binomial_weights (x_weight, x_rwt);
			compute_binomial_weights (y_weight, y_rwt);
		}

		/*for (int i = 0; i < 2*rwt+1; i++) {
                System.err.printf(" %8.6f ", weight[i]);
        }
        System.err.printf("\n");*/

		if (normalize_image) {
			get_grey_image (resize, grey);
			normalize_grey_image (grey, width, height, x_weight, x_rwt, y_weight, y_rwt, grel);
		}
		else {
			get_grey_image (resize, grel);
		}

		
		/*Character distribution*/
		double[][] interval = null;
		if (use_interval) {
			int tmp_rwt = choose_gaussian_weight_size(1.0*height);
			double[] dist_weight = new double[2*tmp_rwt+1];
			compute_gaussian_weights (dist_weight, tmp_rwt);
			interval = distribution (width, height, dist_weight, tmp_rwt, grel);
			//System.err.printf("top-bottom edge detection window size %d\n", tmp_rwt);
		}
		else {
			interval = new double[width][2];
			for (int kx = 0; kx < width; kx++) {
				interval[kx][0] = height-1;
				interval[kx][1] = 0;
			}	
		}
		
		if (debug) {
			System.err.printf("norm window size %d by %d\n", x_rwt, y_rwt);
			double[] tmp = new double[n];
			for (int i = 0; i < n; i++) { tmp[i] = grel[i]; }
			for (int x = 1; x < (width - 1); x++) {
				
				int top = (int)(interval[x][0]+0.5) * width + x;
				int bot = (int)(interval[x][1]+0.5) * width + x;
				if ( (top > 0) && (top < (width*height))) {
					tmp[top] = -2;
				}
				if ( (bot > 0) && (bot < (width*height))) {
					tmp[bot] = -2;
				}
			}

			Util.writeDoubletoPGM (tmp, width, height, -2.0, 2.0, "top_bot"); 
		}
		
		/*Number of Cells*/
		int number_of_cells = number_of_cells_x * number_of_cells_y;

		int total_bins = number_of_cells*bins_per_cell;

		//System.err.printf("number of cells : %d, n : %d\n", number_of_cells, n);
		double[][] cell_weight_image = new double[number_of_cells][n];
		
		/*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;
				
				double[] grad = new double[2];
                                 
				//gradient_sobel (grel, width, height, x, y, grad);
				gradient_simple (grel, width, height, x, y, grad);
                
                dx[position] = grad[0];
                
                dy[position] = grad[1];

				/*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 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));
							
				double r_bot = interval[x][0];
				
				double r_top = interval[x][1];

				if (option > 0) {
					for (int cx = 0; cx < number_of_cells_x; cx++) {
						
						double wtx = cell_weight (option, number_of_cells_x, cx, x, width, r_bot, r_top);
						
						for (int cy = 0; cy < number_of_cells_y; cy++) {
												
						    double wty = cell_weight (option, number_of_cells_y, cy, y, height, r_bot, r_top);
							
							int c_pos = cy * number_of_cells_x + cx;

							int bin_pos = c_pos * bins_per_cell + bin;

							cells_histogram[bin_pos] += dnorm[position]*wtx*wty;
							
							cell_weight_image[c_pos][position] = wtx*wty;
						}
					}
				}
				else {
					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_pos = c_pos * bins_per_cell + bin;
					cells_histogram[bin_pos] += dnorm[position];
					
					for (int cp = 0; cp < number_of_cells; cp++) {
						cell_weight_image[cp][position] = (cp==c_pos?1.0:0.0);
					}
					
				}  
			}
		}

		/*Normalize the histogram of each cell to unit L1 or 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 = 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 += (normalize_histogram_L1 ? v : v * v);
				}

				double cell_norm = (normalize_histogram_L1 ? sum + 1.0e-100: Math.sqrt(sum + 1.0e-100));

				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) {
			double dnorm_max = 0.5;
			
			Util.writeDoubletoPGM (grel, width, height, -2.0, 2.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, dnorm_max, "dnorm");
			
			Util.writePolartoPNG (dnorm, dtheta, width, height, dnorm_max, 0.0, Math.PI, "dpolar");
			
			Util.writeDoubletoPGM (dtheta, width, height, 0, Math.PI, "dtheta");
			
			for (int cp = 0; cp < number_of_cells; cp++) {
				Util.writeDoubletoPGM (cell_weight_image[cp], width, height, 0.0, 1.0, "cwt_"+ String.format("%02d", cp));
			}

			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");
			}
			
			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];
					}
					print_histogram (resize, 
							         histogram, 
							         number_of_cells_x, 
							         number_of_cells_y, 
							         bins_per_cell, 
							         normalize_histogram_L1, 
							         "hog_directions_" + cy + "_" + cx + ".png");
				}
			}
		}

		return cells_histogram;
	}
	
	public int choose_gaussian_weight_size (double dev) {
		double rwt = 2*dev;
		return (int)(Math.ceil(rwt));
	}
	
	public void gradient_sobel (double[] image, int width, int height, int x, int y, double[] grad) {

		assert( (x >= 1) && (x < width-1));
		assert( (y >= 1) && (y < height-1));

		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;
	}
	
	public void gradient_simple (double[] image, int width, int height, int x, int y, double[] grad) {

		int position = y * width + x;
		int kxm = position - 1;
		int kxp = position + 1;
		int kym = position - width;
		int kyp = position + width;

		grad[0] = (image[kxp] - image[kxm])/2;

		grad[1] = (image[kyp] - image[kym])/2;

	}
	
	/*Computes the cell weight factor for one axis {z} (x or y). Given: number of cells 
	 * {ncz}, cell index {cz}, pixel index {z} and number of pixels {npz}, all along
	 * that axis. The option selects the weight type. Adjusting the interval by
	 * {r_bot, r_top}.*/
	public double cell_weight (int option, int ncz, int cz, int z, int npz, double r_bot, double r_top) {
		
		if (ncz == 1) {
			return 1;
		}
		
		double z_star = (z - r_top)/(r_bot - r_top);
		
		if (option == 1) {
			return StepFunc (ncz,cz,z_star);
		}
		else if (option == 2){
			return Bernstein (ncz-1,cz,z_star);
		}
		else if (option == 3) {
			return EdgeCore (ncz,cz,z_star);
		}
		else if (option == 4){
			return Exp (ncz,cz,z_star);
		}
		else {
			assert(false);
			return 0.0;
		}
	}
	
	public void print_histogram (
			BufferedImage image,
			double[] histogram, 
			int number_of_cells_x, 
			int number_of_cells_y, 
			int bins_per_cell, 
			boolean normalize_histogram_L1,
			String name ) {
		
		int width = 600;
		
		int height = 600;

		double magnify = (normalize_histogram_L1 ? Math.sqrt(bins_per_cell) : 1) * 150;

		double radius = 90;
		
		double c_x = width/2;
		
		double c_y = height/2;
		
		int n = width * height;
		
		double shift = 10;
				
		BufferedImage temp = new BufferedImage(width, height, BufferedImage.TYPE_INT_RGB);
		
		Graphics2D g = temp.createGraphics();
		
		g.setStroke(new BasicStroke(1.0f));		
		
		//g.setColor(Color.black);
		float[] hsbvals = new float[3];
		Color.RGBtoHSB(0, 0, 0, hsbvals);
		g.setColor(Color.getHSBColor(hsbvals[0], hsbvals[1], hsbvals[2]));
		
		Graphics2D grey_weak = temp.createGraphics();
		grey_weak.setStroke(new BasicStroke(1.0f));		
		Color.RGBtoHSB(173, 173, 173, hsbvals);
		grey_weak.setColor(Color.getHSBColor(hsbvals[0], hsbvals[1], hsbvals[2]));
		
		Graphics2D grey_dark = temp.createGraphics();
		grey_dark.setStroke(new BasicStroke(1.0f));	
		Color.RGBtoHSB(0, 0, 0, hsbvals);
		grey_dark.setColor(Color.getHSBColor(hsbvals[0], hsbvals[1], hsbvals[2]));
		
		/*White painting*/
		int white = 0xFFFFFF;
		for (int i = 0; i < n; i++) {
			int x = i % width;
			int y = i / width;
			temp.setRGB(x, y, white);
		}

		
		//drawCircle (c_x, c_y, radius, g); //radius = 50 pixels
		
		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;
	
			//double theta_a = ((bin-0.5)*Math.PI)/bins_per_cell - 1;
			
			//double theta_b = ((((bin+1)%bins_per_cell)-0.5)*Math.PI)/bins_per_cell - 1;
	
			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 );
			
			
					
			if ((bin % 2) == 0) {
				grey_dark.fillPolygon(polygon);
			}
			else {
				grey_dark.fillPolygon(polygon);
			}
			}
			
			/*draw_String (
					 c_x, 
					 c_y, 
					 radius, 
					 1.0,
					 bin,
					 bins_per_cell,
					 g);*/
			
			/*draw_String (
					 c_x, 
					 c_y, 
					 radius, 
					 histogram[bin]*magnify,
					 bin,
					 histogram[bin],
					 bins_per_cell,
					 g);*/

			//g.drawString(String.format("%.2f", histogram[bin]), (int)(x2), (int)(y2));
			
			
		}
		grey_weak.dispose();
		grey_dark.dispose();
		g.dispose();
		
		/*Writing cells*/
		BufferedImage img_temp = Util.imageScale (image, 1.0);
		Graphics2D g_image = img_temp.createGraphics();
		g_image.setStroke(new BasicStroke(1.0f));		
		g_image.setColor(Color.white);
		int div_x = img_temp.getWidth()/number_of_cells_x;
		int div_y = img_temp.getHeight()/number_of_cells_y;
		for (int cy = 0; cy < number_of_cells_y; cy++) {
			for (int cx = 0; cx < number_of_cells_x; cx++) {
				g_image.drawLine(div_x*(cx+1), 0, div_x*(cx+1), img_temp.getHeight());
			}
			g_image.drawLine(0, div_y*(cy+1), img_temp.getWidth(), div_y*(cy+1));
		}
		g_image.dispose();
		
		try {
			ImageIO.write(temp, "png", new File(name));
			ImageIO.write(img_temp, "png", new File("division.png"));
		}
		catch (Exception e) { 
				System.err.printf("cannot write image\n");
		}
		
	} 
	
	
	
		private void draw_String (
			 double c_x, 
			 double c_y, 
			 double radius, 
			 double shift,
			 int bin,
			 int nbins,
			 Graphics2D g) { 
		
        double shift_bin = bin + 0.5;	
		
		double theta = (shift_bin * Math.PI)/nbins - 1;
			
		double x_string = c_x + (radius*shift) * Math.cos(theta);

		double y_string = c_y + (radius*shift) * Math.sin(theta);
		
		g.drawString(String.format("%d", bin), (int)x_string, (int)y_string);

		
	}
	
	private void draw_String (
			double c_x, 
			double c_y, 
			double radius, 
			 double shift,
			 int bin,
			 double value,
			 int nbins,
			 Graphics2D g) { 
		
        double shift_bin = bin + 0.5;	
		
		double theta = (shift_bin * Math.PI)/nbins - 1;
			
		double x_string = c_x + (radius*shift) * Math.cos(theta);

		double y_string = c_y + (radius*shift) * Math.sin(theta);
		
		g.drawString(String.format("%.2f", value), (int)x_string, (int)y_string);

		
	}
	
	private Polygon draw_Polygon (
			double c_x, 
			double c_y, 
			double radius, 
			 double theta_a, 
			 double theta_b, 
			 double value,
			double magnify) { 
		
		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);
		
		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;
	}
	
	private void drawCircle (int x, int y, int radius, Graphics2D g){
		g.drawOval(x - radius, y - radius, radius*2, radius*2);
	}

	/*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); }
		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 BernsteinExp (int n, int k, double z) {
		double mu = 0.02;
		double sigma_A = 0.1+mu;
		double sigma_B = 0.1;
		assert((k >= 0) && (k <= n));
		double res = 1.0;
		if (z <= 0) { return (k == 0 ? Math.exp(-(z+mu)*(z+mu)/(2*sigma_A)) : 0); }
		else if (z >= 1) { return (k == n ? Math.exp(-(1+mu-z)*(1+mu-z)/(2*sigma_A)) : 0); }
		else {
			for (int i = 0; i < k; i++) {
				res = (res * (n - i))/(i+1)*z;
			}
			return res*Math.pow(1-z,n-k);
		}
	}
	
	public double Bernstein (int n, int k, double z) {
		assert((k >= 0) && (k <= n));
		double res = 1.0;
		if (z <= 0) { return (k == 0 ? 1 : 0); }
		else if (z >= 1) { return (k == n ? 1 : 0); }
		else {
			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);
		}
	}
	
	public double Exp (int n, int k, double z) {
		double mu = 0.25;
		//double sigma_A = 0.3+mu;
		//double sigma_B = 0.3;
		
		double sigma_A = 0.1+mu;
		double sigma_B = 0.1;
		
		assert((n==1) || (n == 3));
		assert((k >= 0) && (k < n));
		
		if (k == 0) { 
			return Math.exp(-(z+mu)*(z+mu)/(2*sigma_A));
		}
		else if (k == 2) {
			return Math.exp(-(1+mu-z)*(1+mu-z)/(2*sigma_A));
		}
		else if (k == 1) {
			return Math.exp(-(z-0.5)*(z-0.5)/(2*sigma_B));
		}
		else {
			assert(false);
			return 0;
		}
	}
	
	/*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_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);
		}
	}
	
	public void normalize_grey_image (
			double[] grey, 
			int w, 
			int h, 
			double[] x_weight, 
			int x_rwt, 
			double[] y_weight,  
			int y_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, 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);
				//grel[position] = (grey[position] - AVG)/(3*DEV) + 0.5;
				grel[position] = (grey[position] - AVG)/DEV;
				if (grel[position] < 0) { grel[position] = 0.0; }
				else if (grel[position] > 1) { grel[position] = 1.0; }
			}
		}
	
	}
	
	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;
	}
	
	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 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; 
				}

			}
		}
		double noise = 0.01; /*Assumed standard deviation of noise*/
		return Math.sqrt(sum_v2wt/sum_wt + noise*noise);
	}
	

	public double[][] distribution (int w, int h, double[] weight, int rwt, double[] grel) {
		
		double[] s = new double[h];
		
		double[][] intervals = 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_interval (h, s);
			intervals[x][0] = tmp[0];
			intervals[x][1] = tmp[1];

		}
		return intervals;
	}
	
	public double[] get_interval (int h, double[] s)		
	{
		double pct = 0.2;
		
		double[] interval = 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] <= pct*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] <= pct*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);

		interval[0] = Math.min(y_med + y_delta,h-1); //bot
		interval[1]	= Math.max(y_med - y_delta,0); //top 

		return interval;
	}
	
	public double[] get_interval2 (int h, double[] s)		
	{
		double[] interval = 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);

		interval[0] = Math.min(y_med + y_delta,h); //bot
		interval[1]	= Math.max(y_med - y_delta,0); //top 
		
		return interval;
	}

}