package com.elphel.imagej.orthomosaic;

import java.awt.Color;
import java.awt.Font;
import java.io.File;
import java.io.IOException;
import java.io.ObjectInputStream;
import java.io.ObjectOutputStream;
import java.io.Serializable;
import java.nio.charset.StandardCharsets;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.text.SimpleDateFormat;
import java.time.LocalDateTime;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Calendar;
import java.util.HashMap;
import java.util.List;
import java.util.Properties;
import java.util.concurrent.atomic.AtomicInteger;

import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.common.GenericJTabbedDialog;
import com.elphel.imagej.common.PolynomialApproximation;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.gpu.TpTask;
import com.elphel.imagej.ims.Imx5;
import com.elphel.imagej.readers.ElphelTiffReader;
import com.elphel.imagej.readers.ImagejJp4Tiff;
import com.elphel.imagej.tileprocessor.ImageDtt;
import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;
import com.elphel.imagej.tileprocessor.QuadCLT;

import Jama.Matrix;
import ij.ImagePlus;
import ij.ImageStack;
import ij.Prefs;
import ij.gui.PointRoi;
import ij.plugin.filter.AVI_Writer;
import ij.plugin.filter.GaussianBlur;
import ij.process.FloatProcessor;
import ij.process.ImageConverter;
import ij.process.ImageProcessor;
import loci.formats.FormatException;

public class OrthoMap implements Comparable <OrthoMap>, Serializable{
	private static final long serialVersionUID = 1L;
	public static boolean FIX_VERT_Y = false; // true; // temporarily fix vertical Y coordinate bug (use -GCORR in the filename?)
	public static int gpu_width =  4096;
	public static int gpu_height =  4096;
	public static final String [] KEY_DIRS= {"rootDirectory", // from EyesisCorrectionParameters
			"sourceDirectory","linkedModels","videoDirectory","x3dDirectory","resultsDirectory"};
	public static final String ALT_SUFFIX = "-ALT";
//	public transient final String               name;  // timestamp
//	public transient final double               ts;
	public transient String                     name;  // timestamp
	public transient double                     ts;
	public transient String                     path;        // full path to the model directory (including /vXX?)
	public transient String                     scenes_path; // full path to the model directory (including /vXX?)
	public double []                            lla;      // lat/long/alt
	public LocalDateTime                        dt;
	// affine convert (input) rectified coordinates (meters) relative to vert_meters to source image
	// coordinates relative to vert_meters
	public double [][]                          affine = new double[][] {{1,0,0},{0,1,0}}; // relative to vert_meters[]
	public double                               orig_pix_meters;
	public double []                            vert_meters;   // offset of the image vertical in meters (scale-invariant)
	public int                                  orig_width;
	public int                                  orig_height;
	public transient FloatImageData             orig_image;
	public transient FloatImageData             alt_image;
	public int                                  orig_zoom_level;
	public boolean                              orig_zoom_valid;
	public double                               need_extra_zoom;
	public double                               averageRawPixel = Double.NaN; // measure of scene temperature
	public transient double                     averageImagePixel = Double.NaN; // average image pixel value (to combine with raw)
	transient HashMap <Integer, FloatImageData> images;
	HashMap <Double, PairwiseOrthoMatch>        pairwise_matches;
	
	
	private void writeObject(ObjectOutputStream oos) throws IOException {
		oos.defaultWriteObject();
		oos.writeObject(path);
		oos.writeObject(scenes_path);
		// lla is not transient
		// dt is not transient
		// affine is not transient
		// orig_pix_meters is not transient
		// vert_meters is not transient
		// orig_width is not transient
		// orig_height is not transient
		// orig_image does not need to be saved
		// alt_image does not need to be saved
		// orig_zoom_level is not transient
		// orig_zoom_valid is not transient
		// need_extra_zoom is not transient
		// images is not saved
		// pairwise_matches is not transient
	}

	private void readObject(ObjectInputStream ois) throws ClassNotFoundException, IOException {
		ois.defaultReadObject();
		path = (String) ois.readObject();
		name = getNameFromPath(path);
		ts= Double.parseDouble(name.replace("_", "."));
		scenes_path= (String) ois.readObject();
		// lla is not transient
		// dt is not transient
		// affine is not transient
		// orig_pix_meters is not transient
		// vert_meters is not transient
		// orig_width is not transient
		// orig_height is not transient
		// orig_image was not saved
		// alt_image was not saved
		images = new HashMap <Integer, FloatImageData>(); // field images was not saved
		averageImagePixel = Double.NaN; // average image pixel value (to combine with raw)
		
		// pairwise_matches is not transient
//		pairwise_matches = new HashMap<Double, PairwiseOrthoMatch>();
	}
	
    @Override
    public int compareTo(OrthoMap otherPlayer) {
        return Double.compare(ts, otherPlayer.ts);
    }
	
	public static void setGPUWidthHeight(
			int width,
			int height) {
		gpu_width = width;
		gpu_height = height;		
	}

	public int getWidth() {
		return orig_width;
	}
	public int getHeight() {
		return orig_height;
	}
	
	public int getOriginalZoomLevel() {
		return orig_zoom_level;
	}
// Generate ALT image path from the GEO
	public static String getAltPath(String path) {
		int p1 = path.lastIndexOf(".");
		return path.substring(0,p1)+ALT_SUFFIX+".tiff";
	}
	public static String getNameFromPath(String path) {
		int p1 = path.lastIndexOf(Prefs.getFileSeparator());
		if (p1 < 0) return null;
		int p2 = path.indexOf("-", p1+1);
		if (p2 < 0) return null;
		return path.substring(p1+1, p2);
	}
	
	/**
	 * Gets average pixel value of all sensors of the reference scene 
	 * @return
	 */
	public double getTemperature() {
		if (Double.isNaN(averageRawPixel)) {
			final String TIFF_EXT=".tiff";
			final File [] raw_files = (new File(scenes_path)).listFiles();
			final Thread[] threads = ImageDtt.newThreadArray();
			final AtomicInteger ai = new AtomicInteger(0);
			final AtomicInteger ati = new AtomicInteger(0);
			final double [] avg_arr = new double [threads.length];
			final double [] npix_arr = new double [threads.length];
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					public void run() {
						int thread_num = ati.getAndIncrement();
						ImagejJp4Tiff imagejJp4Tiff = new ImagejJp4Tiff();						
						for (int iFile = ai.getAndIncrement(); iFile < raw_files.length; iFile = ai.getAndIncrement()) {
							if (raw_files[iFile].getName().endsWith(TIFF_EXT)) {
								ImagePlus imp = null;
								String spath = raw_files[iFile].toString();
	                			try {
	                				imp= imagejJp4Tiff.readTiffJp4(spath);
	                			} catch (IOException e) {
	                				System.out.println("getImagesMultithreaded IOException " + spath);
	                			} catch (FormatException e) {
	                				System.out.println("getImagesMultithreaded FormatException " + spath);
	                			}
	                			if (imp != null) {
	                				npix_arr[thread_num] = imp.getWidth()*imp.getHeight();
	                				float [] fpixels = (float[]) imp.getProcessor().getPixels();
	                				for (int i = 0; i < fpixels.length; i++) {
	                					avg_arr[thread_num] += fpixels[i];
	                				}
	                				avg_arr[thread_num] /= npix_arr[thread_num];
	                			}
							}
						}
					}
				};
			}		      
			ImageDtt.startAndJoin(threads);
			double avg=0, num=0;
			for (int i = 0; i < avg_arr.length; i++) {
				avg+=avg_arr[i]*npix_arr[i];
				num+=npix_arr[i];
			}
			averageRawPixel = avg/num;
		}
		return averageRawPixel;	
	}
	
	
	public OrthoMap (
			String path,
			String scenes_path) {
		this.path = path;
		this.scenes_path = scenes_path;
		name = getNameFromPath(path);
		ts= Double.parseDouble(name.replace("_", "."));
		Properties imp_prop = null;
		try {
			imp_prop = ElphelTiffReader.getTiffMeta(path);				
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		lla = ElphelTiffReader.getLLA(imp_prop);
		dt = ElphelTiffReader.getLocalDateTime(imp_prop);
		vert_meters =      ElphelTiffReader.getXYOffsetMeters(imp_prop);
		orig_pix_meters = ElphelTiffReader.getPixelSize(imp_prop)[0];
		if (FIX_VERT_Y) {
			int height_pix =   ElphelTiffReader.getHeight(imp_prop);
			double height_meters = height_pix * orig_pix_meters;
			vert_meters[1] = height_meters-vert_meters[1];
		}
		orig_width =  ElphelTiffReader.getWidth(imp_prop);
		orig_height = ElphelTiffReader.getHeight(imp_prop);
		
		orig_zoom_level = FloatImageData.getZoomLevel(orig_pix_meters);
		orig_zoom_valid = FloatImageData.isZoomValid(orig_pix_meters);
		need_extra_zoom = FloatImageData.needZoomIn(orig_pix_meters);
		images = new HashMap <Integer, FloatImageData>();
		pairwise_matches = new HashMap<Double, PairwiseOrthoMatch>();
		averageRawPixel = Double.NaN; // measure of scene temperature
		averageImagePixel = Double.NaN; // average image pixel value (to combine with raw)
	}
	
	
	public LocalDateTime getLocalDateTime() {
		return dt;
	}
	
	public String getName() {
		return name;
	}
	public void setAffine(double [][] affine) {
		this.affine = affine;
	}
	public double [][] getAffine(){
		return affine;
	}

	public void removeImageData() { // to save memory
		orig_image=null;
	}
	
	public void removeAltData() { // to save memory
		alt_image = null;
	}
	
	public void removeZoomLevel(int zoom_level) { // either to save memory or remove to later replace
		images.remove(zoom_level);
	}
	
	public int [] getZoomLevels() {
		Integer [] ks = (Integer[]) images.keySet().toArray();
		int [] zoom_levels = new int[ks.length];
		for (int i = 0; i < ks.length; i++) {
			zoom_levels[i] = ks[i];
		}
		return zoom_levels;
	}
	
	public void removeAllButZoomLevel(int zoom_level) { // either to save memory or remove to later replace
		int [] zoom_levels = getZoomLevels();
		for (int zl:zoom_levels) if (zl !=zoom_level) {
			images.remove(zl);
		}
	}

	
	public FloatImageData getImageData() {
		if (orig_image == null) {
			readImageData();
		}
		getAveragePixel();
		return orig_image;
	}
	public FloatImageData getAltData() {
		if (alt_image == null) {
			readAltData();
		}
		return alt_image;
	}
	
	private boolean readImageData() {
		ImagePlus imp = new ImagePlus(path);
		int width = imp.getWidth();
		int height = imp.getHeight();
		
		// TODO: FIXME!
//		vert_meters[1] = height-vert_meters[1];
		orig_image = new FloatImageData (
				width, // int       width,
				height, // int       height,
				orig_zoom_level, // int       zoom_lev,
				vert_meters, // double [] vert, // x,y pixel offset of the point under the camera
				(float[]) (imp.getProcessor().getPixels())); //    data);
		if (orig_zoom_valid) {
			images.put(orig_zoom_level, orig_image);
		}
		averageImagePixel = Double.NaN;
		return true;
	}

	public double getAveragePixel() {
		if (Double.isNaN(averageImagePixel)) {
			if (orig_image == null) {
				readImageData();
			}
			final float [] pixels =  orig_image.data;
			final Thread[] threads = ImageDtt.newThreadArray();
			final AtomicInteger ai = new AtomicInteger(0);
			final AtomicInteger ati = new AtomicInteger(0);
			final double [] avg_arr =  new double [threads.length];
			final double [] npix_arr = new double [threads.length];
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					public void run() {
						int thread_num = ati.getAndIncrement();
						for (int ipix = ai.getAndIncrement(); ipix < pixels.length; ipix = ai.getAndIncrement()) {
							float p = pixels[ipix];
							if (!Float.isNaN(p)) {
								avg_arr[thread_num] += p;
								npix_arr[thread_num] +=1;
							}
						}
					}
				};
			}		      
			ImageDtt.startAndJoin(threads);
			double avg=0, num=0;
			for (int i = 0; i < avg_arr.length; i++) {
				avg+=avg_arr[i]; // *npix_arr[i];
				num+=npix_arr[i];
			}
			averageImagePixel = avg/num; 
		}
		return averageImagePixel;
	}
	
	
	private boolean readAltData() { // assuming the same scale as main image
		ImagePlus imp = new ImagePlus(getAltPath(path));
		int width = imp.getWidth();
		int height = imp.getHeight();
		alt_image = new FloatImageData (
				width, // int       width,
				height, // int       height,
				orig_zoom_level, // int       zoom_lev,
				vert_meters, // double [] vert, // x,y pixel offset of the point under the camera
				(float[]) (imp.getProcessor().getPixels())); //    data);
		return true;
	}
	
	public ImagePlus getOriginalImage(boolean show_markers) {
		FloatImageData orig_image = getImageData();
		if (orig_image != null) {
			String full_name = path.substring(path.lastIndexOf(Prefs.getFileSeparator()) + 1);
			ImagePlus imp = ShowDoubleFloatArrays.makeArrays(
					orig_image.data, // float[] pixels,
					getWidth(),
					getHeight(),
					full_name);
			if (show_markers) {
				PointRoi roi = new PointRoi();
				roi.addPoint(orig_image.vert[0],orig_image.vert[1]);
				roi.setOptions("label");
				imp.setRoi(roi);
			}
			return imp;
		}
		return null;
	}
	
	public double [] enuOffsetTo(OrthoMap other) {
		return Imx5.enuFromLla(other.lla, lla);
	}
	
	public static double getPixelSizeMeters (int zoom_level ) {
		double ps = 0.01; // 1 cm;
		if (zoom_level >0) {
			for (int i = 0; i < zoom_level; i++) {
				ps /= 2;
			}
		} else {
			for (int i = 0; i > zoom_level; i--) {
				ps *= 2;
			}
		}
		return ps;
	}
	//	private double [][]               affine = new double[2][3];
	// vert_meters =     ElphelTiffReader.getXYOffsetMeters(imp_prop);
	
	
	/**
	 * Get top-left, top-right, bottom-right, bottom-left corners coordinates relative
	 * to a "vertical" point. {{-,-},{+,-},{+,+},{-,+}} 
	 * @return double [4][2] array [corner number]{x,y} 
	 */
	public double [][] get4SourceCornersMeters(){
		FloatImageData orig_image = getImageData();
		double width_meters = getWidth() * orig_pix_meters;
		double height_meters = getHeight() * orig_pix_meters;
		return new double[][] { // CW from TL
			{             - vert_meters[0],               - vert_meters[1]},
			{width_meters - vert_meters[0],               - vert_meters[1]},
			{width_meters - vert_meters[0], height_meters - vert_meters[1]},
			{             - vert_meters[0], height_meters - vert_meters[1]}};
	}
	
	/**
	 * Get metric bounds of this image (zero point at vert_meters) with the affine transform
	 * stored with this image.
	 * @param rectified if true, use rectified (inverse-transformed) image, false - original  
	 * @return rectified {{x_min, x_max},{y_min,y_max}} 
	 */
	public double [][] getBoundsMeters (boolean rectified){
		return getBoundsMeters (rectified, affine);
	}
	
	/**
	 * Get metric bounds of this image (zero point at vert_meters) with specified affine transform
	 * @param rectified if true, use rectified (inverse-transformed) image, false - original
	 * @param affine - 6-element affine transform  
	 * @return rectified {{x_min, x_max},{y_min,y_max}} 
	 */
	public double [][] getBoundsMeters (
			boolean     rectified,
			double [][] affine){
		double [][] corners = get4SourceCornersMeters();
		if (rectified) {
			double [][] inv_aff = invertAffine(affine);
			double [][] rec_corners = new double [4][2];
			for (int n = 0; n < rec_corners.length; n++) {
				rec_corners[n][0] = inv_aff[0][0]*corners[n][0] + inv_aff[0][1]*corners[n][1] + inv_aff[0][2]; 
				rec_corners[n][1] = inv_aff[1][0]*corners[n][0] + inv_aff[1][1]*corners[n][1] + inv_aff[1][2]; 
			}
			corners = rec_corners;
		}
		double [][] bounds = new double [2][2]; // rows: x,y. Columns: min,max.
		for (int n = 0; n < 2; n++) {
			bounds[n][0] = corners[0][n];
			bounds[n][1] = bounds[n][0];
			for (int i = 1; i < corners.length; i++) {
				bounds[n][0]=Math.min(bounds[n][0], corners[i][n]);
				bounds[n][1]=Math.max(bounds[n][1], corners[i][n]);
			}
		}
		return bounds;
	}
	
	
	
	
	/**
	 * Get pixel bounds of this image (zero point at vert_meters) as doubles (to be able to
	 * offset before converting to int.
	 * @param rectified if true, use rectified (inverse-transformed) image, false - original  
	 * @return rectified {{x_min, x_max},{y_min,y_max}} 
	 */
	public double [][] getBoundPixels(
			int zoom_level,
			boolean rectified){
		double [][] bounds_meters = getBoundsMeters (rectified);
		double pix_size = getPixelSizeMeters (zoom_level);
		double [][] bounds_pix = new double[2][2];
		for (int n = 0; n < bounds_pix.length; n++) {
			for (int i = 0; i < 2; i++) {
				bounds_pix[n][i] = bounds_meters[n][i]/pix_size;
			}
		}
		return bounds_pix;
	}
	
	/**
	 * In preparation for the GPU correlation (zoomed out images of the fixed
	 * size are calculated separately and have NaN for unused pixels.
	 * Calculate overlap area bounds relative to the reference image origin
	 * (vertical point) and top-left (of the rectified overlap area) referenced
	 * affine matrices for both images ([2][2][3]).
	 * The overlap area should fit into gpu_width, gpu_hight, the affine-generated
	 * image coordinates will be limited during TpTask[][] generation 
	 * @param ref_map    OrthoMap instance for the reference map
	 * @param var_map    OrthoMap instance for the variable map instance
	 * @param zoom_lev   zoom level (0 - 1 pix=1cm, 1 - 1 pix = 0.5cm, -1 - 1 pix = 2 cm.
	 * @param var_offset additional (to affine and lla, vert_meters) x,y offset of
	 *                   the var_map (in output pixels). Used for spiral search for initial
	 *                   match.
	 * @param gpu_affine combined [2][2][3] array for convertion from the rectified overlap
	 *                   area to the GPU input images. Referenced to the top-left pixel
	 *                   of the rectified overlap area and top-left image pixels
	 * @return {{min_x,max_x}{min_y, max_y}) of the rectified area where (0,0) corresponds
	 *         (is centered) to the reference image origin (vertical point)  
	 */
	public static int [][] getPairOvelapBoundsAffine(
			OrthoMap ref_map,
			OrthoMap var_map,
			int      zoom_lev,
			int []   var_offset,
			double [][][] gpu_affine){
		return null;
	}

	/**
	 * Convert from the affine matrix that calculates image coordinates from the rectified one
	 * (relative to the origin point) to the matrix that provides rectified coordinates from the
	 * image ones. Both are in meters relative to the "vert_meters" point
	 * @param affine [2][3] array were the last column is made of the offsets (in meters)
	 * @return [2][3] array that converts from the image metric coordinates relative to the
	 * "vert_meters" point to the rectified metric coordinates relative to the same point.  
	 */
	public static double [][] invertAffine(double [][] affine){
		Matrix A = new Matrix (
				new double [][] {{affine[0][0],affine[0][1]},{affine[1][0],affine[1][1]}});
		Matrix Ai = A.inverse();
		Matrix B = new Matrix(new double [][] {{affine[0][2]},{affine[1][2]}});
		Matrix AiB = Ai.times(B);
		double [][] iaffine = {
				{Ai.get(0,0),Ai.get(0,1), -AiB.get(0,0)},
				{Ai.get(1,0),Ai.get(1,1), -AiB.get(1,0)}};
		return iaffine;
	}
	
	public boolean downScaleForGPU (int zoom_level){
		FloatImageData orig_image = getImageData();
		if (orig_image == null) {
			System.out.println("Original image is null");
			return false;
		}
		if (!orig_zoom_valid) {
			System.out.println("Original zoom level is invalid, need_extra_zoom = "+need_extra_zoom);
			return false;
		}
		if (zoom_level > orig_zoom_level) {
			System.out.println("Requested zoom level is too high ("+zoom_level+" > "+orig_zoom_level+")");
			return false;
		}
		if (images.get(zoom_level) != null) {
			return true; // already exists
		}
		int rscale = 1;
		for (int i = orig_zoom_level; i > zoom_level; i--) {
			rscale *= 2;
		}
		final int frscale = rscale;
		int swidth =  getWidth();
		int sheight = getHeight();
		final float [] spix = orig_image.data;
		final int width = (swidth+frscale-1)/frscale;
		final int height = (sheight+frscale-1)/frscale;
		final int tiles = width * height;
		final float [] opix = new float [tiles];
		Arrays.fill(opix, Float.NaN);
		final double [][] wnd = new double [frscale] [frscale];
		for (int i = 0; i <frscale; i++) {
			double w = Math.sin((i+0.5)/frscale*Math.PI);
			for (int j = 0; j <frscale; j++) {
				wnd[i][j] = w*Math.sin((j+0.5)/frscale*Math.PI);
			}
		}
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
        for (int ithread = 0; ithread < threads.length; ithread++) {
            threads[ithread] = new Thread() {
                public void run() {
                    for (int tile = ai.getAndIncrement(); tile < tiles; tile = ai.getAndIncrement()) {
                    	int tileX = tile%width;
                    	int tileY = tile/width;
                    	int tileW= (frscale*(tileX+1) <= swidth) ? frscale:(swidth - frscale*tileX);  
                    	int tileH= (frscale*(tileY+1) <= sheight) ? frscale:(sheight - frscale*tileY);
                    	double sw = 0.0, swd = 0.0;
                    	for (int py = 0; py < tileH; py++) {
                    		int ls = (tileY*frscale + py)*swidth;
                    		for (int px = 0; px < tileW; px++) {
                    			double d = spix[ls + tileX*frscale+px];
                    			if (!Double.isNaN(d)) {
                    				double w = wnd[py][px];
                    				sw += w;
                    				swd += w*d;
                    			}
                    			if (sw > 0) {
                    				opix[tileY * width + tileX] = (float) (swd/sw);
                    			}
                    		}
                    	}
                    }
                }
            };
        }		      
        ImageDtt.startAndJoin(threads);
        images.put(zoom_level, new FloatImageData (
				width, // int       width,
				height, // int       height,
				zoom_level, // int       zoom_lev,
				vert_meters, // double [] vert, // x,y pixel offset of the point under the camera
				opix)); //    data);
		return true;
	}
	
	/**
	 * Get elevation map with the same resolution as vector_field corresponding to the GPU-processed
	 * data. Array is organized at a tile grid (width corresponding to GPU width in tiles),source 
	 * tp_tasks provides tile center coordinates for the current GPU zoom level, they are scaled for
	 * the source elevation data
	 * @param zoom_level zoom level of the GPU image, 0 is 1pix=1cm, -1 is 1pix=2cm
	 * @param tp_tasks array of non-empty tiles to be processed in the GPU. Integer tx and ty reference
	 *        tile on a tile grid (8x8 pix), center X,Y for the only "sensor" 0 - float coordinates
	 *        corresponding to the current GPU zoom level
	 * @param tilesX
	 * @param tilesY
	 * @param debugLevel
	 * @return
	 */
	public double [] getTileElevations(
			final int zoom_level,
			final TpTask [] tp_tasks, 
			final int tilesX,
			final int tilesY,
			final int debugLevel) {
		final FloatImageData src_elev = getAltData();
		if (src_elev == null) {
			if (debugLevel > -4) {
				System.out.println("getTileElevations(): No elevation data available for "+name);
			}
			return null;
		}
		final int src_width = src_elev.width;
		final int src_height = src_elev.height;
		final float [] felev = src_elev.data; 
		final double [] elev = new double [tilesX*tilesY];
		Arrays.fill(elev, Double.NaN);
		final int diff_zoom_lev = zoom_level-orig_zoom_level;
//   	double s = 1.0;
//		int zl = diff_zoom_lev;
//		for (; zl< 0; zl++) s *= 2.0;
//		for (; zl> 0; zl--) s *= 0.5; // normally will not happen
		final double scale = 1.0/getScale (diff_zoom_lev); //	int zoom_lev);
		
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int iTile = ai.getAndIncrement(); iTile < tp_tasks.length; iTile = ai.getAndIncrement()) {
						TpTask task = tp_tasks[iTile];
						float [] xy = task.getXY()[0];
						double px = xy[0] * scale; 
						double py = xy[1] * scale;
						// linear interpolate
						int ipx = (int) Math.floor(px);
						int ipy = (int) Math.floor(py);
						double fpx = px -ipx;
						double fpy = py -ipy;
						if ((ipx >= 0) && (ipy >= 0) && (ipx < src_width) && (ipy < src_height)) {
							int indx00 = ipy*src_width+ipx;
							int indx = task.getTileY()*tilesX + task.getTileX();
							elev[indx] =
									(1.0-fpx)*(1.0-fpy)*felev[indx00] +
									(    fpx)*(1.0-fpy)*felev[indx00 + 1] +
									(1.0-fpx)*(    fpy)*felev[indx00 + src_width] +
									(    fpx)*(    fpy)*felev[indx00 + src_width + 1];
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return elev;
	}
	
	/**
	 * Get scale from zoom level: 0-> 1.0, -1 -> 0.5
	 * @param zoom_lev
	 * @return
	 */
	public static double getScale (
			int zoom_lev) {
		double s = 1.0;
		int zl = zoom_lev;
		for (; zl< 0; zl++) s *= 0.5;
		for (; zl> 0; zl--) s *= 2.0;
		return s;
	}
	
	/**
	 * Copy prepared scaled image to the top-left corner of the fixed-size
	 * float[] array for the GPU input
	 * @param zoom_level
	 * @return
	 */
	public float [] getPaddedGPU (
			int zoom_level){
		boolean got_image = downScaleForGPU (zoom_level); // will create if does not exist
		if (!got_image) {
			System.out.println("getPaddedGPU(): failed to prepare a scaled image");
			return null;
		}
		FloatImageData fid = images.get(zoom_level);
		
		final float [] padded_gpu = new float[gpu_width*gpu_height];
		final int swidth =  fid.width;
		final int sheight = fid.height;
		final float [] spix = fid.data;
		Arrays.fill(padded_gpu, Float.NaN);
		final int cheight = Math.min(sheight,gpu_height);
		final int cwidth =  Math.min(swidth, gpu_width);

		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
        for (int ithread = 0; ithread < threads.length; ithread++) {
            threads[ithread] = new Thread() {
                public void run() {
                    for (int row = ai.getAndIncrement(); row < cheight; row = ai.getAndIncrement()) {
        				System.arraycopy(
        						spix,
        						row*swidth,
        						padded_gpu,
        						row*gpu_width,
        						cwidth);
                    }
                }
            };
        }		      
        ImageDtt.startAndJoin(threads);
		return padded_gpu;
	}
	
	
	public static void testVideo(ImagePlus imp) {
		String path_prefix = "/media/elphel/SSD3-4GB/lwir16-proc/ortho_videos/videos/video_";
		boolean   keep_original =        true;
		boolean   um_apply =             true;
		double    um_sigma =             10.0;
		double    um_weight =            0.9;
		boolean   auto_range =           false;
		double    mono_range =           300.0; 
		boolean   annotate   =           true;
		Color     annotate_color =       new Color( 255, 180,  50);
		boolean   annotate_transparent = false;
		int       annotate_font_size =   36;
		boolean   annotate_font_bold =   true;
		double    video_fps =            10;
		boolean   avi_compress_jpeg =    false;
		int       aviJpegQuality =       95;
		boolean   run_ffmpeg =           true;
		String    video_ext =            ".webm";
		String    video_codec =          "vp8";
		int       video_crf =            10; // lower - better, larger file size
		boolean   remove_avi =           true;
		int       debugLevel =           0;
		GenericJTabbedDialog gd = new GenericJTabbedDialog("Parameters for video from orthographic imges",1200,800);
		gd.addStringField ("Output video path prefix",  path_prefix, 180,
				"Absolute path prefix for the video (image title will be added).");
		gd.addCheckbox    ("Keep original image", keep_original, "If true, will generate new RGB image, if false - replace existent.");
		gd.addCheckbox    ("Apply unsharp mask filter", um_apply, "Apply unsharp mask filter.");
		gd.addNumericField("Unsharp mask sigma", um_sigma,  3,7,"",	"Unsharp mask sigma.");
		gd.addNumericField("Unsharp mask weight", um_weight,  3,7,"",	"Unsharp mask weight.");
		gd.addCheckbox    ("Auto range while congerting to RGB", auto_range, "Autorange 32-bit floating point image when converting to RGB.");
		gd.addNumericField("Fixed range during conversion to RGB", mono_range,  3,7,"",	"Fixed range.");
		gd.addCheckbox    ("Annotate each frame with timestamp+date/time", annotate,
				"Embed annotation in the bottom-right corner of each frame of the converted image file and video.");
		{
			String scolor = (annotate_color==null)?"none":String.format("%08x", IntersceneMatchParameters.getLongColor(annotate_color));
			gd.addStringField ("Annotation color (hex number)",scolor, 8, "Any invalid hex number disables annotation");
		}
		gd.addCheckbox    ("Annotation transparent background", annotate_transparent, "Put annotation text directly over images.");
		gd.addNumericField("Annotation font size", annotate_font_size, 0, 4, "", "Annotation font size.");
		gd.addCheckbox    ("Bold annotation", annotate_font_bold, "Use bold font for annotations.");
		gd.addNumericField("Video output frame rate", video_fps,  3,7,"fps", "Frame rate of the video.");
		gd.addCheckbox    ("Compress AVI with JPEG", avi_compress_jpeg, "Use JPEG for AVI compression (false - use raw).");
		gd.addNumericField("AVI JPEG quality", aviJpegQuality, 0, 4, "", "AVI JPEG quality if JPEG compression is used.");
		gd.addCheckbox    ("Convert AVI to WEBM", run_ffmpeg, "Use ffmped to convert intermediate AVI video to WEBM.");
		gd.addStringField ("WEBM output extension",  video_ext, 5,"WEBM output file extension including dot, normally \".webm\".");
		gd.addStringField ("WEBM codec",  video_codec, 5,"WEBM codec \"vp8\" or \"vp9\"(vp9 had problems).");
		gd.addNumericField("WEBM CRF", video_crf, 0, 4, "", "WEBM compression quality (lower - better, 10 - good).");
		gd.addCheckbox    ("Remove AVI", remove_avi, "Remove AVI (large file) after WEBM generation.");
		gd.addNumericField("Debug level", debugLevel, 0, 4, "", "Debug level (not yet used).");
		gd.showDialog();
		if (gd.wasCanceled()) return;
		path_prefix =          gd.getNextString();
		keep_original =        gd.getNextBoolean();
		um_apply =             gd.getNextBoolean();
		um_sigma=              gd.getNextNumber();
		um_weight=             gd.getNextNumber();
		auto_range =           gd.getNextBoolean();
		mono_range =           gd.getNextNumber();
		annotate =             gd.getNextBoolean();
        {
			String scolor =             gd.getNextString();
			long lcolor = -1;
			try {
				lcolor = Long.parseLong(scolor,16);
				annotate_color = IntersceneMatchParameters.setLongColor(lcolor);
			} catch(NumberFormatException e){
				annotate_color = null;
			}
        }
		annotate_transparent =     gd.getNextBoolean();
		annotate_font_size = (int) gd.getNextNumber();		
		annotate_font_bold =       gd.getNextBoolean();
		video_fps =                gd.getNextNumber();
		avi_compress_jpeg =        gd.getNextBoolean();
		aviJpegQuality =     (int) gd.getNextNumber();
		run_ffmpeg =               gd.getNextBoolean();
		video_ext =                gd.getNextString();
		video_codec =              gd.getNextString();
		video_crf =          (int) gd.getNextNumber();
		remove_avi =               gd.getNextBoolean();
		debugLevel =         (int) gd.getNextNumber();
		
		ImagePlus imp_um;
		if (um_apply) {
			imp_um = applyUnsharpMask(
					imp,           // final ImagePlus imp,
					keep_original, // final boolean   keep_original,
					um_sigma,      // 10,            // final double    um_sigma,
					um_weight);    // 0.9);          // final double    um_weight
		} else {
			imp_um = keep_original? imp.duplicate():imp;
		}
        System.out.println("applyUnsharpMask() DONE!");	
    	ImagePlus imp_rgb = convertToColorAnnotate(
    			imp_um,               // ImagePlus imp_src,
    			false,                // boolean   keep_original,
    			auto_range? 0:mono_range, // 300,  // double    mono_range,  // >0 - apply 
    			annotate,             // boolean   annotate,
    			annotate_color,       // Color.RED,  //Color    annotate_color,
    			annotate_transparent, // boolean   annotate_transparent,
    			annotate_font_size,   // int       annotate_font_size, // 12
    			annotate_font_bold,   // boolean   annotate_font_bold,
    			debugLevel);          // int       debugLevel)
    	imp_rgb.show();
        System.out.println("convertToColorAnnotate() DONE!");
        String path = path_prefix+imp_rgb.getTitle();
        int mode_avi = avi_compress_jpeg? 1 : 0;
        saveRGBSlicesAsVideo(
        		imp_rgb,        // ImagePlus  imp,
//    			0,              // int        col_mode, // 0 - mono, 1 - color
    			path,           // String     path,
    			video_fps,      // double     video_fps,
    			mode_avi,       // int        mode_avi, // 0 - raw, 1 - JPEG, 2 - PNG
    			aviJpegQuality, // int        aviJpegQuality,
    			run_ffmpeg,     // boolean    run_ffmpeg,
    			video_ext,      // ".webm",   // String     video_ext,
    			video_codec,    // "vp8",     // String     video_codec,
    			video_crf,      // 10,        // int        video_crf, // lower - better, larger file size
    			remove_avi,     // boolean    remove_avi,
    			debugLevel);    // final int  debugLevel
    			        
        System.out.println("saveRGBSlicesAsVideo() DONE!");
	}
	
	
	public static ImagePlus applyUnsharpMask(
			final ImagePlus imp_src,
			final boolean   keep_original, 
			final double    um_sigma,
			final double    um_weight
			) {
		final float fum_weight=(float) um_weight;
		final ImagePlus imp = keep_original? imp_src.duplicate() : imp_src;
		final ImageStack fstack_scenes = imp.getStack();
		final int nSlices = fstack_scenes.getSize();
		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
		final AtomicInteger ai = new AtomicInteger(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nSlice = ai.getAndIncrement(); nSlice < nSlices; nSlice = ai.getAndIncrement()) {
						FloatProcessor fp = (FloatProcessor) fstack_scenes.getProcessor(nSlice+1);
						float [] fpixels = (float[]) fstack_scenes.getPixels(nSlice+1);
						float [] fpixels_orig = fpixels.clone();
						(new GaussianBlur()).blurFloat(
								fp,       // FloatProcessor ip,
								um_sigma, // double sigmaX,
								um_sigma, // double sigmaY,
								0.01);    // double accuracy)
						for (int i = 0; i < fpixels.length; i++) {
							fpixels[i] = fpixels_orig[i] - fum_weight * fpixels[i];
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		imp.setTitle(removeKnownExtension(imp.getTitle())+String.format("-UM%.1f_%.2f",um_sigma,um_weight));
		return imp;
	}
	
	public static ImagePlus convertToColorAnnotate(
			ImagePlus imp_src,
			boolean   keep_original,
			double    mono_range,  // >0 - apply 
			boolean   annotate,
			Color     annotate_color,
			boolean   annotate_transparent,
			int       annotate_font_size, // 12
			boolean   annotate_font_bold,
			int       debugLevel) {
		final ImagePlus imp = keep_original? imp_src.duplicate() : imp_src;
		if (imp.getType() != ImagePlus.COLOR_RGB) {
			if (mono_range > 0) {
				imp.getProcessor().setMinAndMax(-mono_range/2, mono_range/2);
			}
			ImageConverter imageConverter = new ImageConverter(imp);
			imageConverter.convertToRGB();
		}
		final Color fcolor = annotate_color;
		final ImageStack fstack_scenes = imp.getImageStack();
		int title_len = 0;
		final int nSlices = fstack_scenes.getSize();
		for (int i = 0; i < nSlices; i++) {
			if (fstack_scenes.getSliceLabel(i+1).length() > title_len) {
				title_len=fstack_scenes.getSliceLabel(i+1).length();
			}
		}
		final int annotate_size = title_len; //
		final int width =  imp_src.getWidth();
		final int height = imp_src.getHeight();
		final double char_wtoh = 0.6;
		final int posX= width - ((int) (char_wtoh* annotate_size * annotate_font_size)); //  119; // 521;
		final int posY= height + 1;  // 513;
		final Font font = new Font("Monospaced", Font.PLAIN | (annotate_font_bold ? Font.BOLD: 0), annotate_font_size);
		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
		final AtomicInteger ai = new AtomicInteger(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nSlice = ai.getAndIncrement(); nSlice < nSlices; nSlice = ai.getAndIncrement()) {
						String scene_title = fstack_scenes.getSliceLabel(nSlice+1);
						ImageProcessor ip = fstack_scenes.getProcessor(nSlice+1);
						ip.setColor(fcolor); // Color.BLUE);
						ip.setFont(font);
						if (!annotate_transparent) { // toRGB || !annotate_transparent_mono) {
							ip.drawString(scene_title, posX, posY,Color.BLACK);
						} else {
							ip.drawString(scene_title, posX, posY); // transparent
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		imp.setTitle(removeKnownExtension(imp.getTitle())+((mono_range > 0) ? String.format("_%.0f",mono_range):"_A"));
		return imp;
	}
	
	public static String removeKnownExtension(String path) {
		String [] remove_ext = {".tiff", ".tif", ".avi"};
		for (String ext:remove_ext) {
			if (path.endsWith(ext)) {
				path = path.substring(0,path.length()-ext.length());
			}
		}
		return path;

	}
	public static boolean saveRGBSlicesAsVideo(
			ImagePlus  imp,
//			int        col_mode, // 0 - mono, 1 - color
			String     path,
			double     video_fps,
			int        mode_avi, // compression
			int        aviJpegQuality,
			boolean    run_ffmpeg,
			String     video_ext,
			String     video_codec,
			int        video_crf,
			boolean    remove_avi,
			final int  debugLevel
			) {
		boolean generate_mapped = true; // false - here just simulate
		// no_combine, stereo_2_images, stereo_anaglyth
		String avi_path = removeKnownExtension(path)+String.format("-FPS%04.1f",video_fps)+".avi";
		imp.getCalibration().fps = video_fps;
		video:
		{
			try {
				(new AVI_Writer()).writeImage (
						imp, // ImagePlus imp,
						avi_path, // String path,
						mode_avi, // int compression,
						aviJpegQuality); //int jpegQuality)
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
				break video;
			}
			System.out.println("saveAVIInModelDirectory(): saved "+avi_path);
			if (!run_ffmpeg) {
				break video; // webm not requested
			}

			String webm_path = avi_path.substring(0, avi_path.length()-4)+video_ext;
			// added -y not to as "overwrite y/n?"
			//ffmpeg -i input_file.mkv -c copy -metadata:s:v:0 stereo_mode=1 output_file.mkv 
			//https://ffmpeg.org/ffmpeg-formats.html
			//ffmpeg -i sample_left_right_clip.mpg -an -c:v libvpx -metadata stereo_mode=left_right -y stereo_clip.webm
			//anaglyph_cyan_red 
			String stereo_meta = ""; // used in OpticalFlow
			String shellCommand = String.format("ffmpeg -y -i %s -c %s -b:v 0 -crf %d %s %s",
					avi_path, video_codec, video_crf, stereo_meta, webm_path);
			Process p = null;
			if (generate_mapped) {
				int exit_code = -1;
				try {
					p = Runtime.getRuntime().exec(shellCommand);
				} catch (IOException e) {
					System.out.println("Failed shell command: \""+shellCommand+"\"");
				}
				if (p != null) {
					try {
						p.waitFor();
					} catch (InterruptedException e) {
						System.out.println("saveRGBSlicesAsVideo(): failed waiting for process to finish.");
						// TODO Auto-generated catch block
						e.printStackTrace();
						return false;
					}
					exit_code = p.exitValue();
				}
				System.out.println("Ran shell command: \""+shellCommand+"\" -> "+exit_code);
				// Check if webm file exists
				if ((exit_code != 0) || !(new File(webm_path)).exists()) {
					System.out.println("Failed to create : \""+webm_path+"\"");
					break video;
				}
			} else {
				System.out.println("Simulated shell command: \""+shellCommand);
			}
			if (remove_avi && generate_mapped) {
				(new File(avi_path)).delete();
				System.out.println("Deleted AVI video file: \""+avi_path+"\"");
			}
		}
		return true;
	}
	public static double [][] combineAffine(
			double [][] ref_affine,
			double [][] other_affine){
		Matrix m_ref =   new Matrix(ref_affine);
		Matrix m_other = new Matrix(other_affine);
		Matrix A1 =  m_ref.getMatrix(0,1,0,1);
		Matrix A2 =m_other.getMatrix(0,1,0,1);
		Matrix B1 =  m_ref.getMatrix(0,1,2,2);
		Matrix B2 =m_other.getMatrix(0,1,2,2);
		Matrix A = A2.times(A1);
		Matrix B = A2.times(B1).plus(B2);
		double [][] affine = {
				{A.get(0,0),A.get(0,1),B.get(0,0)}, 
				{A.get(1,0),A.get(1,1),B.get(1,0)}};
		return affine;
	}
	
	
	/**
	 * Get planar approximation of the ground
	 * 
	 * @param zoom_lev      zoom level of the provided elevation data (0- 1pix=1 cm, -1: 1 pix=2cm)
	 * @param elev          elevation data, meters ASL
	 * @param width         pixel width corresponding to the elev[] array
	 * @param initial_above meters above average to ignore for the histogram.
	 * @param initial_below meters below average (positive value) to ignore for the histogram
	 * @param num_refine    number of the plane generation refine passes
	 * @param frac_above    remove high fraction of all tiles
	 * @param frac_below    remove low fraction of all tiles
	 * @param debug_title   Name of the debug image or null to skip
	 * 
	 * @return plane parameters - all metric, relative to the vertical point:
	 *         {tilt_x (m/m), tilt_y (m/m), offset (elevation at vertical point in meters)      
	 */
	public double [] getPlaneMeters (
			int       zoom_lev, // of the data (3 levels down for tiles)
			double [] elev,
			int       width,
			double    initial_above, // from average
			double    initial_below, // from average, // positive value
			int       num_refine,
			double    frac_above,
			double    frac_below,
			String    debug_title) {
		final int     num_bins = 1024;
		int diff_zoom_lev = zoom_lev - orig_zoom_level;
		double pix_size = orig_pix_meters / getScale(diff_zoom_lev); // zoom_lev negative, pixel larger
		double [] pix_vertical = {vert_meters[0]/pix_size,vert_meters[1]/pix_size}; // image center in pixels

		double avg_all = getMaskedAverage (
				elev, // final double [] data,
				null); // final boolean [] mask)
		double abs_high= avg_all + initial_above;
		double abs_low=  avg_all - initial_below;
		boolean [] mask = 	removeAbsoluteLowHigh (
				elev,     // final double [] data,
				null,     // final boolean [] mask,
				abs_high, // final double threshold_high,
				abs_low); // final double threshold_low),
		double [] plane_pix = {0,0,avg_all,pix_vertical[0], pix_vertical[1]}; // 5 elements here
		for (int ntry = 0; ntry < num_refine; ntry++) {
			if (ntry > 0) {
				// remove relatives, start from new mask
				mask = removeRelativeLowHigh (
						elev,       // final double [] data,
						null,       // final boolean [] mask_in,
						initial_above,   //  final double abs_high,
						-initial_below,    // final double abs_low,
						frac_above, // final double rhigh,
						frac_below, // final double rlow,
						plane_pix,  // final double []  ground_plane, // tiltx,tilty, offs, x0(pix), y0(pix) or null
						width,      // final int     width, // only used with ground_plane != null;
						num_bins);  // final int    num_bins) 				
				
			}
			plane_pix=  getPlane(
					elev, // final double []   data,
					mask , // final boolean [] mask,
					null, // final double []  weight,
					width, // final int        width,
					pix_vertical); // final double []  xy0)
		}
		if (debug_title != null) {
			String [] dbg_titles = {"elev", "plane", "diff"};
			double [][] dbg_img = new double [dbg_titles.length][elev.length];
			dbg_img[0] = elev;
			for (int pix = 0; pix < elev.length; pix++) {
				int px = pix % width;
				int py = pix / width;
				double x = px - plane_pix[3];
				double y = py - plane_pix[4];
				dbg_img[1][pix] = plane_pix[2]+ x *plane_pix[0] +y * plane_pix[1];
				dbg_img[2][pix] = dbg_img[0] [pix] - dbg_img[1][pix]; 
			}
    		ShowDoubleFloatArrays.showArrays(
    				dbg_img,
    				width,
    				elev.length / width,
    				true,
    				debug_title,
    				dbg_titles);
		}
		return new double [] {plane_pix[0]*pix_size, plane_pix[1]*pix_size, plane_pix[2]}; // no xy0
	}
	
	
	/**
	 * Convert float[] elevation map to double[], optionally scale resolution
	 * down by integer value without interpolation
	 * @param data       original float elevations ASL
	 * @param downscale  integer downscale value
	 * @param orig_width original image width in pixels
	 * @return converted to double [] and optionally scaled down data (width
	 *         and height scaled by integer division)
	 */
	public static double [] scaleElevationResolution(
			final float [] data,
			final int      downscale,
			final int      orig_width) {
		final int orig_height = data.length/orig_width;
		final int width = orig_width / downscale; // floor
		final int height = orig_height / downscale; // floor
		final double [] data_out = new double [width*height];
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
		if (downscale == 1) { // fast, just copy
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					public void run() {
						for (int iPix = ai.getAndIncrement(); iPix < data.length; iPix = ai.getAndIncrement()) {
							data_out[iPix] = data[iPix];
						}
					}
				};
			}		      
		} else {
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					public void run() {
						for (int iPix = ai.getAndIncrement(); iPix < data.length; iPix = ai.getAndIncrement()) {
							int px = iPix % width;
							int py = iPix / width;
							data_out[iPix] = data[px + py*orig_width];
						}
					}
				};
			}		      
		}
		ImageDtt.startAndJoin(threads);
		return data_out;
	}
	/**
	 * Calculate average of a double array, ignore masked out (!mask[i] and NaNs in the data)
	 * @param data double data array
	 * @param mask optional (may be null) boolean mask - ignore data items that have corresponding mask element false
	 * @return average value of the non-NaN and not disabled by mask elements of data[] 
	 */
	private static double getMaskedAverage (
			final double [] data,
			final boolean [] mask) {
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
		final AtomicInteger ati = new AtomicInteger(0);
		final double [] avg_arr = new double [threads.length];
		final double [] npix_arr = new double [threads.length];
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					int thread_num = ati.getAndIncrement();
					for (int iPix = ai.getAndIncrement(); iPix < data.length; iPix = ai.getAndIncrement()) if (!Double.isNaN(data[iPix]) && ((mask == null) || (mask[iPix]))){
						avg_arr[thread_num] += data[iPix];
						npix_arr[thread_num] += 1.0;
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		double avg=0, num=0;
		for (int i = 0; i < avg_arr.length; i++) {
			avg+=avg_arr[i];
			num+=npix_arr[i];
		}
		return avg/num;
	}
	
	/**
	 * Remove double [] data elements that are lower than threshold_low or greater than threshold_high, ignore masked out and NaN elements
	 * @param data double [] input data
	 * @param mask optional (may be null) mask for the data array. Will be modified if provided
	 * @param threshold_high remove higher elements () and NaNs in the data
	 * @param threshold_low remove lower elements () and NaNs in the data
	 * @return updated mask (same instance if not null) that disables (makes false) filtered out data [] elements
	 */
	private static boolean [] removeAbsoluteLowHigh (
			final double [] data,
			final boolean [] mask,
			final double threshold_high,
			final double threshold_low) {
		final boolean [] mask_out = (mask == null) ? new boolean [data.length] : mask;
		if (mask == null) {
			Arrays.fill(mask_out, true);
		}
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int iPix = ai.getAndIncrement(); iPix < data.length; iPix = ai.getAndIncrement()){
						double d = data[iPix];
						if (!((d >= threshold_low) && (d <= threshold_high))) { // removes NaNs too
							mask_out[iPix] = false;
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return mask_out;
	}

	/**
	 * Create mask to disable tiles that have elevations significantly deviating from a
	 * flat surface and from the vertical point, so the vertical deviations may cause horizontal
	 * matching errors.
	 * @param zoom_lev zoom level of the elevation data (0- 1pix=1cm, -1 - 1pix = 2cm, etc.)
	 * @param elev         array (decimated) of metric elevations ASL
	 * @param width        width in pixels represented by elev array.
	 * @param plane_metric metric plane parameters - {tilt_x (m/m), tilt_y (m/m), offset at vertical point(m)}
	 * @param metric_error maximal allowed metric error (mask tiles that may potentially have larger error).
	 * @return boolean [] mask of the same size as elev, where false disables tiles
	 */
	public boolean [] removeHighElevationMismatch (
			int        zoom_lev, // of the data (3 levels down for tiles)
			double []  elev,
			int        width,
			double []  plane_metric, // tiltx,tilty, offs - in meters
			double     metric_error) {
		double     camera_height = lla[2]- plane_metric[2];
		int diff_zoom_lev = zoom_lev - orig_zoom_level;
		double pix_size = orig_pix_meters / getScale(diff_zoom_lev); // zoom_lev negative, pixel larger
		double [] pix_vertical = {vert_meters[0]/pix_size,vert_meters[1]/pix_size}; // image center in pixels
		double     max_r_diff = metric_error * camera_height / pix_size;
		double [] plane_pix =
			{plane_metric[0]/pix_size,plane_metric[1]/pix_size,plane_metric[2],pix_vertical[0],pix_vertical[1]};
		boolean [] mask = removeHighProjection (
				elev, // final double []  data,
				null, // final boolean [] mask_in,    // will be modified if provided
				max_r_diff, // final double     max_r_diff,
				plane_pix, // final double []  ground_plane, // tiltx,tilty, offs, x0(pix), y0(pix) or null
				width); // final int        width)
		return mask;
	}
	
	
	
	/**
	 * Remove specified fractions (0 <= (rlow+rhigh) <= 1.0) of too low  and too high values 
	 * @param data double [] input data
	 * @param mask_in optional (may be null) mask for the data array. Will be modified if provided.
	 * @param abs_high high limit of the histogram (reasonably higher than useful range of the data[])
	 * @param abs_low low limit of the histogram (reasonably lower than useful range of the data[])
	 * @param rhigh fraction of the highest data[] elements to remove
	 * @param rlow fraction of the lowest data[] elements to remove
	 * @param ground_plane null or {tilt_x, tilt_y, offs, x0,y0}, where tilt_x and tilt_y are per pixel.
	 *                     x0, y0 are also in pixels (not meters)
	 * @param width - width of the data[] array. Only used if ground_plane != null                     
	 * @param num_bins number of the histogram bins
	 * @return boolean array of the remaining data elements. Input mask_in array (if not null) is modified too.
	 */
	private static boolean [] removeRelativeLowHigh (
			final double [] data,
			final boolean [] mask_in,
			final double abs_high,
			final double abs_low,
			final double rhigh,
			final double rlow,
			final double []  ground_plane, // tiltx,tilty, offs, x0(pix), y0(pix) or null
			final int     width, // only used with ground_plane != null;
			final int    num_bins) {
		final boolean [] mask = (mask_in == null) ? new boolean [data.length] : mask_in;
		if (mask_in == null) {
			Arrays.fill(mask, true);
		}
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
		final AtomicInteger ati = new AtomicInteger(0);
		final double [][] hist2 = new double [threads.length][num_bins];
		final double scale = num_bins/(abs_high - abs_low);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					int thread_num = ati.getAndIncrement();
					for (int nPix = ai.getAndIncrement(); nPix < data.length; nPix = ai.getAndIncrement()) if (mask[nPix]){
						double d = data[nPix];
						if (!Double.isNaN(d)) {
							if (ground_plane != null) {
								double x = nPix % width - ground_plane[3];
								double y = nPix/width - ground_plane[4];
								double tilt_x = ground_plane[0];
								double tilt_y = ground_plane[1];
								double offs = ground_plane[2];
								d -= x * tilt_x + y * tilt_y + offs;
							}
							int bin = Math.min(Math.max(((int) Math.round((d-abs_low)*scale)), 0), num_bins-1);
							hist2[thread_num][bin] += 1.0; 
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		ai.set(0);
		final double [] hist =    new double [num_bins];
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int bin = ai.getAndIncrement(); bin < num_bins; bin = ai.getAndIncrement()) {
						for (int i = 0; i < hist2.length; i++) {
							hist[bin]+= hist2[i][bin];
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		double sw = 0;
		for (int bin = 0; bin < num_bins; bin++) {
			sw += hist[bin];
		}
		double trlow = sw * rlow;
		double trhigh = sw * rhigh;
		double sh = 0, shp = 0;
		double threshold_high = abs_high;
		for (int bin = num_bins-1; bin>=0; bin--) {
			shp = sh;
			sh += hist[bin];
			if (sh > trhigh) {
				double r = (sh-trhigh)/(sh-shp);
				threshold_high = abs_low + (bin + r)/scale;
				break;
			}
		}
		sh = 0; 
		shp = 0;
		double threshold_low = abs_low;
		for (int bin = 0; bin < num_bins; bin++) {
			shp = sh;
			sh += hist[bin];
			if (sh > trlow) {
				double r = (trlow-shp)/(sh-shp);
				threshold_low = abs_low + (bin + r)/scale;
				break;
			}
		}
		final double fthreshold_low = threshold_low;
		final double fthreshold_high = threshold_high;
		if (ground_plane == null) {
			return removeAbsoluteLowHigh (
					data,           // final double [] data,
					mask,           // final boolean [] mask,
					fthreshold_high,  // final double threshold_high,
					fthreshold_low); // final double threshold_low)
		}
		ai.set(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
//					int thread_num = ati.getAndIncrement();
					for (int nPix = ai.getAndIncrement(); nPix < data.length; nPix = ai.getAndIncrement()) if (mask[nPix]){
						double d = data[nPix];
						if (!Double.isNaN(d)) {
							double x = nPix % width - ground_plane[3];
							double y = nPix/width - ground_plane[4];
							double tilt_x = ground_plane[0];
							double tilt_y = ground_plane[1];
							double offs = ground_plane[2];
							d -= x * tilt_x + y * tilt_y + offs;
							if (!((d >= fthreshold_low) && (d <= fthreshold_high))) {
								mask[nPix] = false;
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return mask;
	}
	
	/**
	 * Remove (mask out) tiles that have high product of distance (in pixels) from the vertiacal point
	 * by the metric difference between the elevation and an approximation plane 
	 * @param data elevation data, decimated to match GPU tile grid. May have NaNs.
	 * @param mask_in null or boolean[] mask same dimension as the data[]/ False disables processing,
	 *                same as NaN in data[]. Will be modified if provided;
	 * @param max_r_diff maximal product of pixel distance from the vertical point and elevation difference
	 *                   from the plane.
	 * @param ground_plane {tilt_x, tilt_y, offs, x0(pix), y0(pix)} or null tilt_x and tilt_y are measured
	 *                     in meters/pixel, offs - in meters and x0,y0 - in pixels
	 * @param width        pixel width of the data[]
	 * @return boolean [] mask with disabled tiles
	 */
	private static boolean [] removeHighProjection (
			final double []  data,
			final boolean [] mask_in,    // will be modified if provided
			final double     max_r_diff,
			final double []  ground_plane, // tiltx,tilty, offs, x0(pix), y0(pix) or null
			final int        width) {
		final double max_r2_diff = max_r_diff*max_r_diff;
		final boolean [] mask = (mask_in == null) ? new boolean [data.length] : mask_in; 
		if (mask_in == null) {
			Arrays.fill(mask, true);
		}
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nPix = ai.getAndIncrement(); nPix < data.length; nPix = ai.getAndIncrement()) if (mask[nPix]){
						double d = data[nPix];
						if (!Double.isNaN(d)) {
							double x = nPix % width - ground_plane[3];
							double y = nPix/width - ground_plane[4];
							double tilt_x = ground_plane[0];
							double tilt_y = ground_plane[1];
							double offs = ground_plane[2];
							d -= x * tilt_x + y * tilt_y + offs;
							double d2 = d*d;
							double r2 = x*x + y*y;
							double d2r2 = d2*r2;
							if (d2r2 > max_r2_diff) {
								mask[nPix] = false;
							}
						} else {
							mask[nPix] = false;
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return mask;
	}
	
	
	
	/**
	 * Fit a plane to the input data, relative to the specified point in Rectangle wnd 
	 * @param data double input data array
	 * @param mask mask that disables somew input data (or null)
	 * @param weight optional weights array (same size as data and mask) 
	 * @param width  width of the rectangle
	 * @param xy0    a pair of x0, y0 - origin where the plane is referenced too
	 * @return double array of {tiltx, tilty, offset, xy0[0] and xy0[1]).
	 */
	private static double [] getPlane(
			final double []   data,
			final boolean [] mask,
			final double []  weight,
			final int        width,
			final double []  xy0) {
		final double [][][] mdatai = new double [data.length][][];
		AtomicInteger anum_good = new AtomicInteger(0);
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nPix = ai.getAndIncrement(); nPix < data.length; nPix = ai.getAndIncrement()) if (mask[nPix]){
						double d = data[nPix];
						if (!Double.isNaN(d)) {
							int x = nPix % width;  
							int y = nPix / width;
							double dx = x-xy0[0];
							double dy = y-xy0[1];
							int mindx = anum_good.getAndIncrement();
							mdatai[mindx] = new double[3][];
							mdatai[mindx][0] = new double [2];
							mdatai[mindx][0][0] = dx;
							mdatai[mindx][0][1] = dy;
							mdatai[mindx][1] = new double [1];
							mdatai[mindx][1][0] =  d; 
							mdatai[mindx][2] = new double [1];
							mdatai[mindx][2][0] =  (weight == null)? 1.0 : weight[nPix];
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		final double [][][] mdata = new double [anum_good.get()][][];
		System.arraycopy(mdatai, 0, mdata, 0, mdata.length);
		double[][] approx2d = (new PolynomialApproximation()).quadraticApproximation(
				mdata,
				true,       // boolean forceLinear,  // use linear approximation
				null,       // damping,    // double [] damping, null OK
				-1);        // debug level
		if (approx2d != null) {
			return new double[] {approx2d[0][0], approx2d[0][1], approx2d[0][2], xy0[0], xy0[1]}; // tiltX, tiltY, offset
		}
		return null;
	}
	
	
	
	
}
