package com.elphel.imagej.tileprocessor;

import java.nio.file.Path;
import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashMap;
import java.util.HashSet;
import java.util.LinkedHashMap;
import java.util.Map;
import java.util.Properties;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.stream.Collectors;

import com.elphel.imagej.calibration.CalibrationFileManagement;
import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.cameras.ColorProcParameters;
import com.elphel.imagej.common.DoubleGaussianBlur;
import com.elphel.imagej.common.PolynomialApproximation;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.cuas.Cuas;
import com.elphel.imagej.ims.Imx5;
import com.elphel.imagej.jp4.JP46_Reader_camera;

import ij.ImagePlus;
import ij.text.TextWindow;

public class GroundPlane {
	public static final String GROUND_PLANE_PREFIX = "groundplane";
	public static final String ELEV_GND_SUFFIX = "-ELEV_GND";
	public double [][] to_ground_xyzatr = null;
	public double []   disparity_plane =  null; // {tiltX, tiltY, offset, fraction_good)
	public double      frac_good =        Double.NaN;  // fraction of "ground" tiles used to find local ground plane
	public double []   lla =              null;        // GPS LLA
	public double []   quat_enu =         null;        // IMS orientation quaternion == d2.getQEnu()
	
	public static GroundPlane getGroundPlaneProperties(String preprefix, Properties properties) {
		GroundPlane gp = new GroundPlane();
		gp.getProperties(preprefix, properties);
		if (gp.to_ground_xyzatr == null) {
			return null;
		}
		return gp;
	}
	public double [][] getToGroundPlane(){
		return to_ground_xyzatr;
	}
	public double [] getDisparityTilts(){
		return disparity_plane;
	}
	public GroundPlane() {}
	public GroundPlane(
			double [][] to_ground_xyzatr,
			double []   disparity_plane,
			double      frac_good,
			double []   lla,
			double []   quat_enu) {
		this.to_ground_xyzatr = new double [][] {to_ground_xyzatr[0].clone(),to_ground_xyzatr[1].clone()}; // will fail if any is null - both needed
		this.disparity_plane = disparity_plane;
		this.frac_good = frac_good;
		this.lla = lla.clone();
		this.quat_enu = quat_enu.clone();
	}
	
	public static Map<String, GroundPlane> getGroundPlanes(
			CLTParameters        clt_parameters,
			ColorProcParameters  colorProcParameters,
			QuadCLT              index_scene,
			QuadCLT []           quadCLTs, // may be null or contain nulls
			boolean              print_rslt, 
			int                  debugLevel){
		QuadCLT quadCLT_main = index_scene; // is it OK?
		HashMap<String, GroundPlane> groundPlanes = new HashMap<String, GroundPlane>();
		if ((index_scene == null) || (index_scene.getRefScenes() == null)) {
			return null;
		}
		for (String ts: index_scene.getRefScenes()) {
			QuadCLT scene = null;
			if (quadCLTs != null) {
				for (QuadCLT s:quadCLTs) if ((s != null) && s.getImageName().equals(ts)) {
					scene = s;
					break;
				}
			}
			if (scene == null) {
				scene = (QuadCLT) quadCLT_main.spawnNoModelQuadCLT( // will conditionImageSet
						ts,
						clt_parameters,
						colorProcParameters,
						ImageDtt.THREADS_MAX,
						debugLevel-2);
			}
			// is it needed or it is done in spawnNoModelQuadCLT()
			scene.restoreInterProperties( // restore properties for interscene processing (extrinsics, ers, ...) // get relative poses (98)
					null, // String path,             // full name with extension or null to use x3d directory
					false, // boolean all_properties,//				null, // Properties properties,   // if null - will only save extrinsics)
					debugLevel);
			groundPlanes.put(ts, scene.getGroundPlane());
		}
		
		Map<String, GroundPlane> sortedMap = groundPlanes.entrySet()
			    .stream()
			    .sorted(Map.Entry.comparingByKey()) // Sort by key
			    .collect(Collectors.toMap(
			        Map.Entry::getKey,
			        Map.Entry::getValue,
			        (oldValue, newValue) -> oldValue, // Merge function for potential duplicates
			        LinkedHashMap::new               // Ensures the result is an ordered LinkedHashMap
			    ));
		if (print_rslt) {
			double [] ims_ortho =     clt_parameters.imp.ims_ortho;
			double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
			System.out.println(String.format("%20s, %15s, %15s, %15s, %15s, %15s, %15s, %15s, %15s, %15s, %15s"+
			",%15s, %15s, %15s, %15s, %15s, %15s, %15s",
					"timestamp", "X", "Y", "Z", "Azimuth", "Tilt", "Roll", "Good",
					"Latitude","Longitude","Altitude",
					"qenu[0]","qenu[1]","qenu[2]","qenu[3]",
					"cam-A","cam-T","cam-R"));
			for (String ts:sortedMap.keySet()) {
				GroundPlane gp = sortedMap.get(ts);
				double [][] xyzatr = gp.to_ground_xyzatr;
				double frac_good = gp.frac_good;
				double [] lla = gp.lla;
				double [] qenu = gp.quat_enu;
				double [] cam_quat_ref_enu =Imx5.quaternionImsToCam(
						qenu ,
						ims_mount_atr, // new double[] {0, 0.13, 0},
						ims_ortho);
				double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu);	
				System.out.println(String.format("%20s, %15.12f, %15.12f, %15.12f, %15.12f, %15.12f, %15.12f, %15.12f"+
						", %15.10f, %15.10f, %15.10f"+
						", %15.12f, %15.12f, %15.12f, %15.12f"+
						", %15.12f, %15.12f, %15.12f",
						
						
						ts, xyzatr[0][0], xyzatr[0][1], xyzatr[0][2], xyzatr[1][0], xyzatr[1][1], xyzatr[1][2],
						frac_good,
						lla[0],lla[1],lla[2],
						qenu[0],qenu[1],qenu[2],qenu[3],
						ref_abs_atr_enu[0],ref_abs_atr_enu[1],ref_abs_atr_enu[2]));
			}
		}
		return sortedMap;
	}
	
	public static void csvGroundPlanes(
			CLTParameters            clt_parameters,
			Map<String, GroundPlane> planes_map,
			QuadCLT                  ref_scene,
			String                   path,
			String                   comment,
			int                      debugLevel) {
			double [] ims_ortho =     clt_parameters.imp.ims_ortho;
			double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
			String header = "timestamp\tX\tY\tZ\tAzimuth\tTilt\tRoll\tGood"+
					"\tLatitude\tLongitude\tAltitude"+
					"\tqenu[0]\tqenu[1]\tqenu[2]\tqenu[3]"+
					"\tcam-A\tcam-T\tcam-R\tdisp-TX\tdisp-TY\tdisp_center";
			StringBuffer sb = new StringBuffer();
			for (String ts:planes_map.keySet()) {
				GroundPlane gp = planes_map.get(ts);
				double [][] xyzatr = gp.to_ground_xyzatr;
				double frac_good = gp.frac_good;
				double [] lla = gp.lla;
				double [] qenu = gp.quat_enu;
				double [] cam_quat_ref_enu =Imx5.quaternionImsToCam(
						qenu ,
						ims_mount_atr, // new double[] {0, 0.13, 0},
						ims_ortho);
				double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu);
				
				sb.append(ts+"\t"+xyzatr[0][0]+"\t"+xyzatr[0][1]+"\t"+xyzatr[0][2]+"\t"+xyzatr[1][0]+"\t"+xyzatr[1][1]+"\t"+xyzatr[1][2]);
				sb.append("\t"+frac_good+"\t"+lla[0]+"\t"+lla[1]+"\t"+lla[2]);
				sb.append("\t"+qenu[0]+"\t"+qenu[1]+"\t"+qenu[2]+"\t"+qenu[3]);
				sb.append("\t"+ref_abs_atr_enu[0]+"\t"+ref_abs_atr_enu[1]+"\t"+ref_abs_atr_enu[2]);
				if (gp.disparity_plane != null) {
					sb.append("\t"+gp.disparity_plane[0]+"\t"+gp.disparity_plane[1]+"\t"+gp.disparity_plane[2]+"\n");
				}
			}
			if (path!=null) {
				String footer=(comment != null) ? ("Comment: "+comment): "";
				CalibrationFileManagement.saveStringToFile (
						path,
						header+"\n"+sb.toString()+"\n"+footer);
				if (debugLevel > -3) {
					System.out.println("Saved ground planes data to "+path);
				}
			} else {
				new TextWindow("Sharpness History", header, sb.toString(), 1000,900);
			}
	}
	
	public void getProperties(String preprefix, Properties properties){ // restore
		String prefix = preprefix+GROUND_PLANE_PREFIX+"_";
		if (properties.getProperty(prefix+"to_ground_xyzatr")!=null) {
			double [] d = ErsCorrection.parseDoublesCSV(properties.getProperty(prefix+"to_ground_xyzatr"));
			to_ground_xyzatr = new double [][] {{d[0],d[1],d[2]},{d[3],d[4],d[5]}};
		}
		if (properties.getProperty(prefix+"disparity_plane")!=null) {
			disparity_plane = ErsCorrection.parseDoublesCSV(properties.getProperty(prefix+"disparity_plane"));
		}
		if (properties.getProperty(prefix+"frac_good")!=null) {
			frac_good = Double.parseDouble(properties.getProperty(prefix+"frac_good"));
		}
		if (properties.getProperty(prefix+"lla")!=null) {
			lla = ErsCorrection.parseDoublesCSV(properties.getProperty(prefix+"lla"));
		}
		if (properties.getProperty(prefix+"quat_enu")!=null) {
			                  quat_enu = ErsCorrection.parseDoublesCSV(properties.getProperty(prefix+"quat_enu"));
		}
	}
	
	public void setProperties(String preprefix, Properties properties){ // save
		String prefix = preprefix+GROUND_PLANE_PREFIX+"_";
		properties.setProperty(prefix+"to_ground_xyzatr",String.format("%f, %f, %f, %f, %f, %f",
				to_ground_xyzatr[0][0],to_ground_xyzatr[0][1],to_ground_xyzatr[0][2],
				to_ground_xyzatr[1][0],to_ground_xyzatr[1][1],to_ground_xyzatr[1][2]));
		if (disparity_plane != null) {
			properties.setProperty(prefix+"disparity_plane",String.format("%f, %f, %f",disparity_plane[0],disparity_plane[1], disparity_plane[2]));
		}
		properties.setProperty(prefix+"frac_good",String.format("%f",frac_good));
		properties.setProperty(prefix+"lla",String.format("%f, %f, %f",lla[0],lla[1], lla[2]));
		properties.setProperty(prefix+"quat_enu",String.format("%f, %f, %f, %f",
				quat_enu[0],quat_enu[1],quat_enu[2],quat_enu[3]));
	}
	
	public static GroundPlane getGroundPlane(
			CLTParameters    clt_parameters,
			final QuadCLT    ref_scene,
			final double []  gnd_disp, // if not null
			final int        debugLevel) {
	
		final boolean [] good_tiles = new boolean[ref_scene.getTilesX()*ref_scene.getTilesY()];
//		String     dbg_title =ref_scene.getImageName()+"-ground_tilts"; 
		String     dbg_title = clt_parameters.imp.fgnd_debug ? (clt_parameters.imp.fgnd_debug_prefix+ "_tilts"): null; // "-ground_tilts"; 
		String     dbg_title_virt = clt_parameters.imp.fgnd_debug ? clt_parameters.imp.fgnd_debug_prefix: null; // "-ground_tilts"; 
		String     dbg_title_disp = null; // clt_parameters.imp.fgnd_debug ? (clt_parameters.imp.fgnd_debug_prefix + "_disp"): null; // "-ground_tilts"; 
		double [][] to_ground_xyzatr_frac= getPlaneDualPassMetric( // returns to_ground_xyzatr (rotated around the ground point nadir of the drone)
				clt_parameters,        // final CLTParameters    clt_parameters,
				ref_scene,             // final QuadCLT    ref_Clt,
				ref_scene.getTilesX(), // final int        width,
				good_tiles,            // final boolean [] good_tiles, // null or boolean[data.length] // should all be false
				gnd_disp,              // final double []  gnd_disp, // if not null
				dbg_title,             // final String     dbg_title,
				debugLevel);           // final int        debugLevel)
		if (to_ground_xyzatr_frac == null) {
			to_ground_xyzatr_frac = new double [][] {new double[3], new double[3], new double[1]};
		}
		// To get terrain disparity (until rotation is fixed) use "good_tiles" from getPlaneDualPassMetric and re-calculate ground disparity plane
		
		double [][] to_ground_xyzatr = {to_ground_xyzatr_frac[0],to_ground_xyzatr_frac[1]};
		double frac_good = to_ground_xyzatr_frac[2][0];
// Until fixed, run independently 		
		
		// Apply good_tiles mask and find disparity plane
		
		double [] disparity_plane = getDisparityPlaneSinglePass( // returns to_ground_xyzatr (rotated around the ground point nadir of the drone)
				clt_parameters,        // final CLTParameters clt_parameters,
				ref_scene,             // final QuadCLT    ref_Clt,
				ref_scene.getTilesX(), // final int        width,
				good_tiles,            // final boolean [] good_tiles, // null or boolean[data.length] // should all be false
				dbg_title_disp,        // final String     dbg_title,
				debugLevel); // final int        debugLevel)
		
		return new GroundPlane(
				to_ground_xyzatr,     // double [][] to_ground_xyzatr,
				disparity_plane,      // double []   disparity_plane,
				frac_good,            // double      frac_good,
				ref_scene.getLla(),   // double []   lla,
				ref_scene.getQEnu()); // double []   quat_enu) {
	}
	
	public static double [][] getPlaneDualPassMetric( // returns to_ground_xyzatr (rotated around the ground point nadir of teh drone)
			final CLTParameters    clt_parameters,
			final QuadCLT    ref_Clt,
			final int        width,
			final boolean [] good_tiles, // null or boolean[data.length] // should all be false
			final double []  gnd_disp, // if not null
			final String     dbg_title,
			final int        debugLevel) {
		boolean print_histogram = debugLevel > 0;
		
		final double   gnd_percent_low = clt_parameters.imp.fgnd_percent_low; // 0.01; // Discard lowest outliers. Percentile to remove lowest elevation outliers during initial terrain filtering
		final double   gnd_percent_high= clt_parameters.imp.fgnd_percent_high; // 0.2;  // Discard highest outliers. Percentile to remove highest elevation outliers during initial terrain filtering.
		final double   gnd_max_high_over=clt_parameters.imp.fgnd_max_high_over; // 0.1;  // Absolute disparity over lowest. Absolute disparity over lowest. Minimal of this value and the one determined by the highest outliers percentile will be applied. // pix = make dependent on average disparity?
//		final int      min_good1 =       clt_parameters.imp.fgnd_min_good1; //10;    // Minimal good tiles in stage 1. Minimal number of remaining tiles after the first stage filtering.
		final double   max_abs_diff =    clt_parameters.imp.fgnd_max_abs_diff; // 0.03; // Absolute disparity from plane. Maximal absolute disparity difference from the stage 1 - found plane (stage2 mask).
		final double   max_rel_diff =    clt_parameters.imp.fgnd_max_rel_diff; // 0.03; // Relative disparity from plane. Maximal relative disparity difference from the stage 1 - found plane (stage2 mask).
		final double   blur_frac  =      clt_parameters.imp.fgnd_blur_frac; // 0.01; // Elevation histogram relative sigma. Elevation (relative to the plane) histogram LPF sigma as a fraction of the full range minimal of the absolute and relative above.
		final double   weight_frac  =    clt_parameters.imp.fgnd_weight_frac; // 0.3;  // Scale outliers from plane weights. Relative to full range disparity from plane to reduce weight twice - multiply remaining tiles weights by w=  1/(1 + (diff_from_plane^2/k_max_diff)^2).
	 	final int      mtile_size  =     clt_parameters.imp.fgnd_mtile_size; //16;    // Macrotile size (50% overlapping). Size of the 50%-overlapping square macrotiles (in tiles).
		final double   min_ev_rel  =     clt_parameters.imp.fgnd_min_ev_rel; // 0.3;  // Minimal macrotile eigenvalues product. Square root of the eigenvalues product. Measure of how well is the macrotile local plane is defined. Fully populated macrotile has it 1.0.
		final double   max_tilt  =       clt_parameters.imp.fgnd_max_tilt; // 0.2;  // Maximal macrotile tilt. Maximal absolute value of the macrotile plane tilt: z to sqrt (x^2+y^2).
		final double   top_percent  =    clt_parameters.imp.fgnd_top_percent; // 0.5;  // High elevation outliers percentile. Remove macrotiles that have the highest elevations (measured at the full camera FOV center).
		final int      min_macro_tiles = clt_parameters.imp.fgnd_min_macro_tiles; // 3;    // Minimal number of macrotiles for high outliers. Minimal number of remaining macrotiles before removing high outpliers.
		final double   normal_damping =  clt_parameters.imp.fgnd_normal_damping; // 0.001; // Damping parameter for planes. Pull planes to horizontal when there is not enough data for tilts.
		
//     	boolean     eig_weight =  false;
		boolean use_lma = true;
		int num_bins = 1000;
		String dbg_title1 = (dbg_title != null) ? (dbg_title + "-stage1") : null;
		String dbg_title2 = (dbg_title != null) ? (dbg_title + "-stage2") : null;
     	double [][] dls = ref_Clt.getDLS();
     	if (dls==null) {
     		return null;
     	}
     	double [][] ds = new double [][] {dls[use_lma?1:0].clone(), dls[2]};
		final double []  ref_disparity = ds[0];
		final double []  ref_strength =  ds[1];
		final double inf_disp_ref = ref_Clt.getDispInfinityRef(); // TODO: change to correction for the whole sequence, not a segment (or combine ?)
		
		if (inf_disp_ref != 0) {
			if (debugLevel >-3) {
				System.out.println("getPlaneDualPass(): applying disparity at infinity correctio, subrtacting "+inf_disp_ref);
			}
			for (int i = 0; i < ref_disparity.length; i++) {
				ref_disparity[i] -= inf_disp_ref;
			}
		}
		
		final int height = ref_disparity.length / width;
		final double [] xy0= {width/2, height/2};
		double avg_val = weightedAverage (
				ref_disparity, // double [] data, // may have NaNs
				ref_strength); // double [] weights)
		double hist_low = 0.0;
		double hist_high = 1.5 * avg_val;
		double [] hist = getHistogram1d(
				ref_disparity,     // double [] data, // may have NaNs
				ref_strength,      // double [] weights,
				hist_low,     // double    hist_low,
				hist_high,    // double    hist_high,
				num_bins,     // int       num_bins,
				debugLevel);  // int       debugLevel)
		double [] fractions = {gnd_percent_low, gnd_percent_high};
		double [] percentiles = getPercentiles(
				hist,          // double [] hist,
				fractions);    // double [] fractions)
		double low_lim =  hist_low + percentiles[0] * (hist_high-hist_low);
		double high_lim = hist_low + percentiles[1] * (hist_high-hist_low);
		if (high_lim > (low_lim + gnd_max_high_over)) {
			high_lim = low_lim + gnd_max_high_over;
		}
		boolean [] mask = new boolean [ref_disparity.length];
		int num_good1=0;
		for (int i = 0; i < mask.length; i++) {
			if ((ref_disparity[i] >= low_lim) && (ref_disparity[i] <= high_lim)) {
				mask[i] = true;
				num_good1++;
			}
		}
		// first mask may be small (if the image was tilted a lot)
		if (debugLevel >-3) {
			System.out.println("getPlaneDualPass(): num_good1="+num_good1+
					" low_lim="+low_lim+" high_lim="+high_lim+" fractions=["+fractions[0]+","+fractions[1]+"]");
		}
		
		double [] tilts_stage1 = getPlane( //  {approx2d[0][0], approx2d[0][1], approx2d[0][2], xy0[0], xy0[1]}; // tiltX, tiltY, offset
				ref_disparity,           // final double []   data,
				mask,           // final boolean [] mask,
				ref_strength,        // final double []  weight,
				width,          // final int        width,
				xy0,            // final double []  xy0,
				normal_damping, // final double     normal_damping,
				ref_Clt,        // final QuadCLT    dbg_scene,
				dbg_title1);    // String dbg_title)
		if (tilts_stage1 == null) {
			System.out.println("getPlaneDualPass(): tilts_stage1= null");
			return null;
		}
		if (debugLevel >-3) {
			System.out.println("getPlaneDualPass() stage1: tiltX="+tilts_stage1[0]+
					" tiltY="+tilts_stage1[1]+" offset="+tilts_stage1[2]+" fraction good1="+(1.0*num_good1/ref_disparity.length)+" num_good1="+num_good1);
		}

		boolean [] mask2 = new boolean [ref_disparity.length];
		int num_good2 = 0;
		double max_diff = Math.min(max_abs_diff, max_rel_diff * low_lim);
		if (debugLevel >-3) {
			System.out.println("getPlaneDualPass() stage1: max_abs_diff="+max_abs_diff+", max_rel_diff="+max_rel_diff+
					", low_lim="+ low_lim+ ", max_diff="+max_diff);
		}
		for (int i = 0; i < ref_disparity.length; i++) {
			int x = i % width;  
			int y = i / width;
			double dx = x-xy0[0];
			double dy = y-xy0[1];
			double plane = tilts_stage1[0] * dx +  tilts_stage1[1] * dy + tilts_stage1[2];
//			data_tilted[i] = ref_disparity[i] - plane;
			double d_tilted = ref_disparity[i] - plane;
			if (Math.abs(d_tilted) <=  max_diff) {
				mask2[i] = true;
				num_good2++;
			}
		}
		if (debugLevel >-3) {
			System.out.println("getPlaneDualPass(): num_good2="+num_good2+ " max_diff="+max_diff);
		}
		
		// re-calculate disparity tilts with mask2
		double [] tilts_stage2 = getPlane( //  {approx2d[0][0], approx2d[0][1], approx2d[0][2], xy0[0], xy0[1]}; // tiltX, tiltY, offset
				ref_disparity,    // final double []   data,
				mask2,            // final boolean [] mask,
				ref_strength,     // final double []  weight,
				width,            // final int        width,
				xy0,              // final double []  xy0,
				normal_damping,   // final double     normal_damping,
				ref_Clt,        // final QuadCLT    dbg_scene,
				dbg_title2);      // String dbg_title)
		if (tilts_stage2 == null) {
			System.out.println("getPlaneDualPass(): tilts_stage2= null");
			return null;
		}
		
		double [] data_tilted = new double[ref_disparity.length];
		for (int i = 0; i < ref_disparity.length; i++) {
			int x = i % width;  
			int y = i / width;
			double dx = x-xy0[0];
			double dy = y-xy0[1];
			double plane = tilts_stage2[0] * dx +  tilts_stage2[1] * dy + tilts_stage2[2];
			data_tilted[i] = ref_disparity[i] - plane;
		}
//		int num_bins2 = 100
		double [] hist2 = getHistogram1d(
				data_tilted,   // double [] data, // may have NaNs
				ref_strength,  // double [] weights,
				-max_diff,     // double    hist_low,
				max_diff,      // double    hist_high,
				num_bins,      // int       num_bins,
				debugLevel);   // int       debugLevel)
		double [] hist2_bkp = print_histogram? hist2.clone() : null;
		(new DoubleGaussianBlur()).blur1Direction(
				hist2,                // double [] pixels,
				hist2.length,         // int        width,
				1,                    // int       height,
				blur_frac * num_bins, // double     sigma,
				0.02,                 // double   accuracy,
				true);                // boolean xDirection
		int hist_max = 0;
		for (int i = 1; i < hist2.length; i++) {
			if (hist2[i] > hist2[hist_max]) {
				hist_max = i;
			}
		}
		if (print_histogram) {
			System.out.println("blur_frac="+blur_frac+", max_diff="+max_diff);
			System.out.println("index, histogram,lpf_histogram");
			for (int i = 0; i < hist2.length; i++) {
				System.out.println(i+", "+hist2_bkp[i]+", "+hist2[i]);
			}
		}
		
		double val = -max_diff + 2 * max_diff * hist_max/hist2.length;
		boolean [] mask3 = new boolean[ref_disparity.length];
		double max_diff2 = max_diff*max_diff;
		double [] weights = ref_strength.clone();
		int num_good3 = 0; 
		for (int i = 0; i < mask3.length; i++) {
			double d = data_tilted[i] - val;
			double k = d/(weight_frac * max_diff);
			weights[i] *= 1.0/(1.0 + k*k);
			if (d*d <  max_diff2) {
				mask3[i] = true;
				num_good3++;
			}
		}
		if (debugLevel >-3) {
			System.out.println("getPlaneDualPass(): num_good3="+num_good3+ " max_diff="+max_diff);
		}
		// go to world metric with the same mask
		
     	double [][] wxyz = OpticalFlow.transformToWorldXYZ(
     			ref_disparity,  // final double []   disparity_ref, // invalid tiles - NaN in disparity
     			ref_Clt,        // final QuadCLT     quadClt, // now - may be null - for testing if scene is rotated ref
     			ImageDtt.THREADS_MAX);   // int               threadsMax)
     	boolean use_parallel_proj = false; // true; // maybe make false to get z-offset to the ground to avoid moving
     	String dbg_title_metric = (dbg_title != null) ?(dbg_title+"-metric"):null;
     	double [][] to_ground_xyzatr = getPlane( // from the camera coordinates to in-plane coordinates
     			wxyz,              // final double [][] wxyz,
     			use_parallel_proj, // final boolean     use_parallel_proj, // for parallel xyz is 0, otherwise - point on the ground under the camera
    			mask3,             // final boolean []  mask,
    			weights,           // final double []   weight,
    			width,             // final int         width,
    			normal_damping,    // final double      normal_damping,
    			dbg_title_metric,  //  final String      dbg_title,
    			debugLevel); // final int         debugLevel) {
     	
     	if (to_ground_xyzatr == null) {
     		return new double [][] {{0,0,0},{0,0,0},{0}};
     	}
     	String dbg_title_macro = (dbg_title != null) ?(dbg_title+"-macroplanes"):null;

     	double [][] macroplanes = getMacroPlanes( // no weights
     			wxyz,                     // final double [][] wxyz,
     			mask3,                     // final boolean []  mask,
     			weights,                   // final double []   weight,
     			false, // final boolean     eig_weight, // use weights when calculating eiget values (false - only mask)
     			width,                     // final int         tilesX,
     			mtile_size,                // final int         mtile_size,
    			normal_damping,            // final double      normal_damping,
    			dbg_title_macro,           //  final String      dbg_title,
    			debugLevel);               // final int         debugLevel) {
     	boolean [] macro_mask =  getMaskFromMacro(
     			mask3,       // boolean  [] mask_in,
     			macroplanes, // double [][] macroplanes,
     			min_ev_rel,  // double      min_ev_rel,
     			max_tilt,    // double      max_tilt,
     			top_percent, // double      top_percent, // remove above percentile
     			min_macro_tiles, // int         min_macro_tiles,
     			width,       // int         tilesX,
     			mtile_size,  // int         mtile_size,
     			debugLevel); // int         debugLevel)
     	
       // re-run with updated mask	
     	to_ground_xyzatr = getPlane( // from the camera coordinates to in-plane coordinates
     			wxyz,              // final double [][] wxyz,
     			use_parallel_proj, // final boolean     use_parallel_proj, // for parallel xyz is 0, otherwise - point on the ground under the camera
     			macro_mask, // mask3,             // final boolean []  mask,
    			weights,           // final double []   weight,
    			width,             // final int         width,
    			normal_damping,    // final double      normal_damping,
    			dbg_title_metric,  //  final String      dbg_title,
    			debugLevel); // final int         debugLevel) {
     	if (to_ground_xyzatr == null) {
     		return new double [][] {{0,0,0},{0,0,0},{0}};
     	}
     	
     	
     	double [][] macroplanes_weights = getMacroPlanes(
     			wxyz,                     // final double [][] wxyz,
     			mask3,                     // final boolean []  mask,
     			weights,                   // final double []   weight,
     			true, // final boolean     eig_weight, // use weights when calculating eigen values (false - only mask)
     			width,                     // final int         tilesX,
     			mtile_size,                // final int         mtile_size,
    			normal_damping,            // final double      normal_damping,
    			dbg_title_macro,           //  final String      dbg_title,
    			debugLevel);               // final int         debugLevel) {
     	
     	double [][] ground_xyzatr = ErsCorrection.invertXYZATR(to_ground_xyzatr); // straight down from the camera, then rotated
     	double [][] virtual_camera_from_ground_xyz = ErsCorrection.invertXYZATR(new double [][] {ground_xyzatr[0],new double[3]});
     	double [][] virtual_camera_from_camera =     ErsCorrection.combineXYZATR(ground_xyzatr, virtual_camera_from_ground_xyz);
     	// FIXME: Next is correct (at least approximately, "-"/invert, order,), find out why
     	System.out.println("FIXME: Inverted X, Y, kept Z, and ATR. Find out why.");
     	/*
     	double [][] vcfs_test = {
     			{virtual_camera_from_camera[0][0], virtual_camera_from_camera[0][1],  virtual_camera_from_camera[0][2]},
     			{ to_ground_xyzatr[1][0],  to_ground_xyzatr[1][1],  to_ground_xyzatr[1][2]}};
		double [][] ivcfs_test =ErsCorrection.invertXYZATR(vcfs_test);
     	*/
		double [][] virtual_camera_from_camera_mod = {
				{-virtual_camera_from_camera[0][0], -virtual_camera_from_camera[0][1], virtual_camera_from_camera[0][2]},
				virtual_camera_from_camera[1] };
		
     	if ((dbg_title != null) || (gnd_disp != null)) {
    		int         around =               clt_parameters.imp.fgnd_virt_around; // 2;
    		double      around_sigma =         clt_parameters.imp.fgnd_virt_sigma; // 4.0;
    		int         num_virtual_refines =  clt_parameters.imp.fgnd_virt_refine; // 4;
    		String      debugSuffix=           (dbg_title != null) ? (dbg_title+"_virtual") : null; //  "virtual_gnd"; // null
    		double [] gnd_disparity = Cuas.getDisparityVirtual(
    				clt_parameters,      // CLTParameters  clt_parameters,
    				ref_disparity,       // double []         disparity_ref,
    				virtual_camera_from_camera_mod, // ivcfs_test,       // final double []   scene_xyz,         // camera center in world (reference) coordinates
    				ref_Clt,             // final QuadCLT     reference_QuadClt,
    				around,              // final int         around,            // 2 search around for interpolation
    				around_sigma,        // final double      sigma,
    				num_virtual_refines, // final int         num_refines,
    				debugSuffix);        // final String      debugSuffix)
     		if (dbg_title != null) {
        		int tilesX = width;
         		int tilesY = wxyz.length/width;
        		int mtile_half = mtile_size/2;
        		int mtilesX = (tilesX - mtile_size)/mtile_half + 1;
        		double [][] macro_exp =  new double [tilesX * tilesY][];
        		double [][] macro_wexp = new double [tilesX * tilesY][];
        		for (int mtile = 0; mtile < macroplanes.length; mtile++){
        			int mtileX = mtile % mtilesX;
        			int mtileY = mtile / mtilesX;
        			int min_y = mtileY * mtile_half + mtile_half/2;
        			int min_x = mtileX * mtile_half + mtile_half/2;
        			int max_y = min_y + mtile_half; 
        			int max_x = min_x + mtile_half;
        			if (min_y < mtile_half) min_y = 0;
        			if (min_x < mtile_half) min_x = 0;
        			if (max_y > (tilesY-mtile_half)) max_y = tilesY;
        			if (max_x > (tilesX-mtile_half)) max_x = tilesX;
        			if (macroplanes[mtile] != null) {
        				for (int tileY = min_y; tileY < max_y; tileY++) {
        					for (int tileX = min_x; tileX < max_x; tileX++) {
        						int ntile = tileY * tilesX + tileX;
        						macro_exp[ntile] = macroplanes[mtile];
        					}
        				}
        			}
        			if (macroplanes_weights[mtile] != null) {
        				for (int tileY = min_y; tileY < max_y; tileY++) {
        					for (int tileX = min_x; tileX < max_x; tileX++) {
        						int ntile = tileY * tilesX + tileX;
        						macro_wexp[ntile] = macroplanes_weights[mtile];
        					}
        				}
        			}
        		}

     			double [][] wxyz_gnd = OpticalFlow.transformToWorldXYZ(
     					gnd_disparity,  // final double []   disparity_ref, // invalid tiles - NaN in disparity
     					ref_Clt,        // final QuadCLT     quadClt, // now - may be null - for testing if scene is rotated ref
     					ImageDtt.THREADS_MAX);   // int               threadsMax)
     			double [] ref_z = new double [ref_disparity.length];
     			double [] gnd_z = new double [ref_disparity.length];
     			Arrays.fill(ref_z, Double.NaN);
     			Arrays.fill(gnd_z, Double.NaN);
     			for (int i = 0; i <ref_disparity.length; i++) {
     				double [] wxyz0 =   wxyz[i];
     				double [] wxyz_g0 = wxyz_gnd[i];
     				if (wxyz0 !=   null) ref_z[i] = wxyz0[2];
     				if (wxyz_g0 != null) gnd_z[i] = wxyz_g0[2];

     			}

     			String [] dbg_titles = {"ref_disparity", "ground_disparity", "ref_z", "gnd_z","masked_z","weights",
     					"offs_macro", "tX_macro", "ty_macro", "eigen_prod", "eigen0", "eigen1", "weigen_prod", "weigen0", "weigen1",
     					"z_macro", // masked ref_z after evaluating macro tiles
     					"z-center"};// masked intersection of camera axis with the macro plane 
     			double [][] dbg_data = new double[dbg_titles.length][];
     			dbg_data[ 0] = ref_disparity;
     			dbg_data[ 1] = gnd_disparity;
     			dbg_data[ 2] = ref_z;
     			dbg_data[ 3] = gnd_z;
     			dbg_data[ 4] = gnd_z.clone();
     			dbg_data[ 5] = weights.clone();
     			dbg_data[ 6] = new double[tilesX*tilesY];
     			Arrays.fill(dbg_data[6],Double.NaN);
     			dbg_data[ 7] = dbg_data[6].clone();  
     			dbg_data[ 8] = dbg_data[6].clone();  
     			dbg_data[ 9] = dbg_data[6].clone();  
     			dbg_data[10] = dbg_data[6].clone();  
     			dbg_data[11] = dbg_data[6].clone();  
     			dbg_data[12] = dbg_data[6].clone();  
     			dbg_data[13] = dbg_data[6].clone();  
     			dbg_data[14] = dbg_data[6].clone();  
     			dbg_data[15] = dbg_data[6].clone();  
     			dbg_data[16] = dbg_data[6].clone();  
     			for (int i = 0; i < mask3.length; i++) {
     				if (!mask3[i]) {
     					dbg_data[4][i] = Double.NaN;		
     					dbg_data[5][i] = Double.NaN;		
     				}
     				if (macro_exp[i] != null) {
     					for (int j = 0; j < macro_exp[i].length; j++) {
     						dbg_data[ 6+j][i] = macro_exp[i][j];		
     					}
     				}
     				if (macro_wexp[i] != null) {
     					for (int j = 0; j < 3; j++) {
     						dbg_data[ 12+j][i] = macro_wexp[i][3+j];		
     					}
     				}
     				if (macro_mask[i]) {
     					dbg_data[15][i] = ref_z[i];		
     					dbg_data[16][i] = dbg_data[ 6][i]; // offset		
     				}
     			}
     			ref_Clt.saveDoubleArrayInModelDirectory(
     					dbg_title+"-result",      // String      suffix,
     					dbg_titles,               // String []   labels, // or null
     					dbg_data,                 // double [][] data,
     					width,                    // int            width, // int tilesX,
     					dbg_data[0].length/width);// int            height, // int tilesY,
     		}
     	}

     	if (good_tiles != null) {
     		System.arraycopy(macro_mask, 0, good_tiles, 0, macro_mask.length);
     	}
		ErsCorrection.printVectors(virtual_camera_from_camera); // to_ground_xyzatr_centered);
		ref_Clt.getErsCorrection().printVectors    (virtual_camera_from_camera[0], virtual_camera_from_camera[1]); 		
     	//num_good2
		return new double[][] {to_ground_xyzatr[0], to_ground_xyzatr[1], {1.0*num_good2/ref_disparity.length}};
	}
	
	public static boolean [] getMaskFromMacro(
			boolean  [] mask_in,
			double [][] macroplanes,
			double      min_ev_rel,
			double      max_tilt,
			double      top_percent, // remove above percentile
			int         min_macro_tiles,
			int         tilesX,
			int         mtile_size,
			int         debugLevel) {
		int        num_bins = 100;
		double     max_tilt2 = max_tilt*max_tilt;
		boolean [] mask = mask_in.clone();
 		int tilesY = mask.length/tilesX;
		int mtile_half = mtile_size/2;
		int mtilesX = (tilesX - mtile_size)/mtile_half + 1;
		boolean [] keep_macro = new boolean [macroplanes.length];
		double [] min_max_macro = null;
		int num_macro = 0;
		double [] macro_weights = new double [macroplanes.length];
		double [] macro_offsets = new double [macroplanes.length];
		double sw = 0, swtx=0, swty=0;
		for (int mtile = 0; mtile < macroplanes.length; mtile++){
			keep_macro[mtile] = macroplanes[mtile] != null;
			if (keep_macro[mtile]) {
				keep_macro[mtile] = macroplanes[mtile][3] > min_ev_rel;
			}
			if (keep_macro[mtile]) {
				double w = macroplanes[mtile][3];
				double tx = macroplanes[mtile][1];
				double ty = macroplanes[mtile][2];
				sw += w;
				swtx += w * tx;
				swty += w * ty;
			}
		}
		if (sw == 0) {
			return mask; // all false
		}
		swtx /= sw;
		swty /= sw;
		for (int mtile = 0; mtile < macroplanes.length; mtile++){
			if (keep_macro[mtile]) {
				double tx = macroplanes[mtile][1] - swtx; // tilt from original plane
				double ty = macroplanes[mtile][2] - swty;
				keep_macro[mtile] = (tx*tx + ty*ty) < max_tilt2;
			}
			if (keep_macro[mtile]) {
				double z = macroplanes[mtile][0];
				if (min_max_macro == null) {
					min_max_macro = new double [] {z, z};
				} else {
					if (z < min_max_macro[0]) min_max_macro[0] = z; 
					if (z > min_max_macro[1]) min_max_macro[1] = z; 
				}
				num_macro++;
				macro_weights[mtile] = macroplanes[mtile][3]; // eigenvalues product
				macro_offsets[mtile] = macroplanes[mtile][0]; //
			}
		}
		if ((num_macro >= min_macro_tiles) && (min_max_macro[1] > min_max_macro[0])) {
			double [] hist = getHistogram1d(
					macro_offsets,     // double [] data, // may have NaNs
					macro_weights,     // double [] weights,
					min_max_macro[0],     // double    hist_low,
					min_max_macro[1],    // double    hist_high,
					num_bins,     // int       num_bins,
					debugLevel);  // int       debugLevel)
			double [] fractions = {top_percent};
			double [] percentiles = getPercentiles(
					hist,          // double [] hist,
					fractions);    // double [] fractions)
			double high_lim = min_max_macro[0] + percentiles[0] * (min_max_macro[1] - min_max_macro[0]);
			// if (high_lim > (low_lim + gnd_max_high_over)) high_lim = low_lim + gnd_max_high_over;
			for (int mtile = 0; mtile < macroplanes.length; mtile++) if (keep_macro[mtile]){
				if (macro_offsets[mtile] > high_lim) {
					keep_macro[mtile] = false;
				}
			}			
		}
		
		for (int mtile = 0; mtile < macroplanes.length; mtile++){
			int mtileX = mtile % mtilesX;
			int mtileY = mtile / mtilesX;
			int min_y = mtileY * mtile_half + mtile_half/2;
			int min_x = mtileX * mtile_half + mtile_half/2;
			int max_y = min_y + mtile_half; 
			int max_x = min_x + mtile_half;
			if (min_y < mtile_half) min_y = 0;
			if (min_x < mtile_half) min_x = 0;
			if (max_y > (tilesY-mtile_half)) max_y = tilesY;
			if (max_x > (tilesX-mtile_half)) max_x = tilesX;
			if (!keep_macro[mtile]) {
				for (int tileY = min_y; tileY < max_y; tileY++) {
					for (int tileX = min_x; tileX < max_x; tileX++) {
						int ntile = tileY * tilesX + tileX;
						mask[ntile] = false;
					}
				}
			} 
		}
		return mask;
	}
	
	
	
	public static double [] getDisparityPlaneDualPass( // returns to_ground_xyzatr (rotated around the ground point nadir of teh drone)
			final CLTParameters clt_parameters,
			final QuadCLT    ref_Clt,
			final int        width,
			final boolean [] good_tiles, // null or boolean[data.length] // should all be false
			final double []  gnd_disp, // if not null
			final String     dbg_title,
			final int        debugLevel) {
		boolean print_histogram = debugLevel > 0;
		
		final double   gnd_percent_low = clt_parameters.imp.fgnd_percent_low; // 0.01; // Discard lowest outliers. Percentile to remove lowest elevation outliers during initial terrain filtering
		final double   gnd_percent_high= clt_parameters.imp.fgnd_percent_high; // 0.2;  // Discard highest outliers. Percentile to remove highest elevation outliers during initial terrain filtering.
		final double   gnd_max_high_over=clt_parameters.imp.fgnd_max_high_over; // 0.1;  // Absolute disparity over lowest. Absolute disparity over lowest. Minimal of this value and the one determined by the highest outliers percentile will be applied. // pix = make dependent on average disparity?
//		final int      min_good1 =       clt_parameters.imp.fgnd_min_good1; //10;    // Minimal good tiles in stage 1. Minimal number of remaining tiles after the first stage filtering.
		final double   max_abs_diff =    clt_parameters.imp.fgnd_max_abs_diff; // 0.03; // Absolute disparity from plane. Maximal absolute disparity difference from the stage 1 - found plane (stage2 mask).
		final double   max_rel_diff =    clt_parameters.imp.fgnd_max_rel_diff; // 0.03; // Relative disparity from plane. Maximal relative disparity difference from the stage 1 - found plane (stage2 mask).
		final double   normal_damping =  clt_parameters.imp.fgnd_normal_damping; // 0.001; // Damping parameter for planes. Pull planes to horizontal when there is not enough data for tilts.
		final double   blur_frac  =      clt_parameters.imp.fgnd_blur_frac; // 0.01; // Elevation histogram relative sigma. Elevation (relative to the plane) histogram LPF sigma as a fraction of the full range minimal of the absolute and relative above.
		final double   weight_frac  =    clt_parameters.imp.fgnd_weight_frac; // 0.3;  // Scale outliers from plane weights. Relative to full range disparity from plane to reduce weight twice - multiply remaining tiles weights by w=  1/(1 + (diff_from_plane^2/k_max_diff)^2).

		boolean use_lma = true;
		int num_bins = 1000;
		String dbg_title1 = (dbg_title != null) ? (dbg_title + "-stage1") : null;
		String dbg_title2 = (dbg_title != null) ? (dbg_title + "-stage2") : null;
		String dbg_title3 = (dbg_title != null) ? (dbg_title + "-stage3") : null;
     	double [][] dls = ref_Clt.getDLS();
     	if (dls==null) {
     		return null;
     	}
     	double [][] ds = new double [][] {dls[use_lma?1:0].clone(), dls[2]};
		final double []  ref_disparity = ds[0];
		final double []  ref_strength =  ds[1];
		final double inf_disp_ref = ref_Clt.getDispInfinityRef();
		
		if (inf_disp_ref != 0) {
			if (debugLevel >-3) {
				System.out.println("getPlaneDualPass(): applying disparity at infinity correctio, subrtacting "+inf_disp_ref);
			}
			for (int i = 0; i < ref_disparity.length; i++) {
				ref_disparity[i] -= inf_disp_ref;
			}
		}
		
		final int height = ref_disparity.length / width;
		final double [] xy0= {width/2, height/2};
		double avg_val = weightedAverage (
				ref_disparity, // double [] data, // may have NaNs
				ref_strength); // double [] weights)
		double hist_low = 0.0;
		double hist_high = 1.5 * avg_val;
		double [] hist = getHistogram1d(
				ref_disparity,     // double [] data, // may have NaNs
				ref_strength,      // double [] weights,
				hist_low,     // double    hist_low,
				hist_high,    // double    hist_high,
				num_bins,     // int       num_bins,
				debugLevel);  // int       debugLevel)
		double [] fractions = {gnd_percent_low, gnd_percent_high};
		double [] percentiles = getPercentiles(
				hist,          // double [] hist,
				fractions);    // double [] fractions)
		double low_lim =  hist_low + percentiles[0] * (hist_high-hist_low);
		double high_lim = hist_low + percentiles[1] * (hist_high-hist_low);
		if (high_lim > (low_lim + gnd_max_high_over)) {
			high_lim = low_lim + gnd_max_high_over;
		}
		boolean [] mask = new boolean [ref_disparity.length];
		int num_good1=0;
		for (int i = 0; i < mask.length; i++) {
			if ((ref_disparity[i] >= low_lim) && (ref_disparity[i] <= high_lim)) {
				mask[i] = true;
				num_good1++;
			}
		}
		// first mask may be small (if the image was tilted a lot)
		if (debugLevel >-3) {
			System.out.println("getPlaneDualPass(): num_good1="+num_good1+
					" low_lim="+low_lim+" high_lim="+high_lim+" fractions=["+fractions[0]+","+fractions[1]+"]");
		}
		
		double [] tilts_stage1 = getPlane( //  {approx2d[0][0], approx2d[0][1], approx2d[0][2], xy0[0], xy0[1]}; // tiltX, tiltY, offset
				ref_disparity,           // final double []   data,
				mask,           // final boolean [] mask,
				ref_strength,        // final double []  weight,
				width,          // final int        width,
				xy0,            // final double []  xy0,
				normal_damping, // final double     normal_damping,
				ref_Clt,        // final QuadCLT    dbg_scene,
				dbg_title1);    // String dbg_title)
		if (tilts_stage1 == null) {
			System.out.println("getPlaneDualPass(): tilts_stage1= null");
			return null;
		}
		if (debugLevel >-3) {
			System.out.println("getPlaneDualPass() stage1: tiltX="+tilts_stage1[0]+
					" tiltY="+tilts_stage1[1]+" offset="+tilts_stage1[2]+" fraction good1="+(1.0*num_good1/ref_disparity.length)+" num_good1="+num_good1);
		}

		boolean [] mask2 = new boolean [ref_disparity.length];
		int num_good2 = 0;
		double max_diff = Math.min(max_abs_diff, max_rel_diff * low_lim);
		if (debugLevel >-3) {
			System.out.println("getPlaneDualPass() stage1: max_abs_diff="+max_abs_diff+", max_rel_diff="+max_rel_diff+
					", low_lim="+ low_lim+ ", max_diff="+max_diff);
		}
		for (int i = 0; i < ref_disparity.length; i++) {
			int x = i % width;  
			int y = i / width;
			double dx = x-xy0[0];
			double dy = y-xy0[1];
			double plane = tilts_stage1[0] * dx +  tilts_stage1[1] * dy + tilts_stage1[2];
//			data_tilted[i] = ref_disparity[i] - plane;
			double d_tilted = ref_disparity[i] - plane;
			if (Math.abs(d_tilted) <=  max_diff) {
				mask2[i] = true;
				num_good2++;
			}
		}
		if (debugLevel >-3) {
			System.out.println("getPlaneDualPass(): num_good2="+num_good2+ " max_diff="+max_diff);
		}
		
		// re-calculate disparity tilts with mask2
		double [] tilts_stage2 = getPlane( //  {approx2d[0][0], approx2d[0][1], approx2d[0][2], xy0[0], xy0[1]}; // tiltX, tiltY, offset
				ref_disparity,    // final double []   data,
				mask2,            // final boolean [] mask,
				ref_strength,     // final double []  weight,
				width,            // final int        width,
				xy0,              // final double []  xy0,
				normal_damping,   // final double     normal_damping,
				ref_Clt,        // final QuadCLT    dbg_scene,
				dbg_title2);      // String dbg_title)
		if (tilts_stage2 == null) {
			System.out.println("getPlaneDualPass(): tilts_stage2= null");
			return null;
		}
		
		double [] data_tilted = new double[ref_disparity.length];
		for (int i = 0; i < ref_disparity.length; i++) {
			int x = i % width;  
			int y = i / width;
			double dx = x-xy0[0];
			double dy = y-xy0[1];
			double plane = tilts_stage2[0] * dx +  tilts_stage2[1] * dy + tilts_stage2[2];
			data_tilted[i] = ref_disparity[i] - plane;
		}
		double [] hist2 = getHistogram1d(
				data_tilted,   // double [] data, // may have NaNs
				ref_strength,  // double [] weights,
				-max_diff,     // double    hist_low,
				max_diff,      // double    hist_high,
				num_bins,      // int       num_bins,
				debugLevel);   // int       debugLevel)
		double [] hist2_bkp = print_histogram? hist2.clone() : null;
		(new DoubleGaussianBlur()).blur1Direction(
				hist2,                // double [] pixels,
				hist2.length,         // int        width,
				1,                    // int       height,
				blur_frac * num_bins, // double     sigma,
				0.02,                 // double   accuracy,
				true);                // boolean xDirection
		int hist_max = 0;
		for (int i = 1; i < hist2.length; i++) {
			if (hist2[i] > hist2[hist_max]) {
				hist_max = i;
			}
		}
		if (print_histogram) {
			System.out.println("blur_frac="+blur_frac+", max_diff="+max_diff);
			System.out.println("index, histogram,lpf_histogram");
			for (int i = 0; i < hist2.length; i++) {
				System.out.println(i+", "+hist2_bkp[i]+", "+hist2[i]);
			}
		}
		
		double val = -max_diff + 2 * max_diff * hist_max/hist2.length;
		boolean [] mask3 = new boolean[ref_disparity.length];
		double max_diff2 = max_diff*max_diff;
		double [] weights = ref_strength.clone();
		int num_good3 = 0; 
		for (int i = 0; i < mask3.length; i++) {
			double d = data_tilted[i] - val;
			double k = d/(weight_frac * max_diff);
			weights[i] *= 1.0/(1.0 + k*k);
			if (d*d <  max_diff2) {
				mask3[i] = true;
				num_good3++;
			}
		}
		if (debugLevel >-3) {
			System.out.println("getPlaneDualPass(): num_good3="+num_good3+ " max_diff="+max_diff);
		}
		
		
		double [] tilts_stage3 = getPlane( //  {approx2d[0][0], approx2d[0][1], approx2d[0][2], xy0[0], xy0[1]}; // tiltX, tiltY, offset
				ref_disparity,    // final double []   data,
				mask3,            // final boolean [] mask,
				weights,          // final double []  weight,
				width,            // final int        width,
				xy0,              // final double []  xy0,
				normal_damping,   // final double     normal_damping,
				ref_Clt,        // final QuadCLT    dbg_scene,
				dbg_title3);      // String dbg_title)
		if (tilts_stage3 == null) {
			System.out.println("getPlaneDualPass(): tilts_stage3= null");
			return null;
		}
		return new double [] {tilts_stage3[0],tilts_stage3[1], tilts_stage3[2], 1.0*num_good3/ref_disparity.length};
	}
	
	/**
	 * After mask is already defined from matric including macrotiles
	 * @param clt_parameters
	 * @param ref_Clt
	 * @param width
	 * @param good_tiles
	 * @param dbg_title
	 * @param debugLevel
	 * @return
	 */
	public static double [] getDisparityPlaneSinglePass( // returns to_ground_xyzatr (rotated around the ground point nadir of teh drone)
			final CLTParameters clt_parameters,
			final QuadCLT    ref_Clt,
			final int        width,
			final boolean [] good_tiles, // null or boolean[data.length] // should all be false
			final String     dbg_title,
			final int        debugLevel) {
		final double   normal_damping =  clt_parameters.imp.fgnd_normal_damping; // 0.001; // Damping parameter for planes. Pull planes to horizontal when there is not enough data for tilts.

		boolean use_lma = true;
		String dbg_title1 = (dbg_title != null) ? (dbg_title + "-stage1") : null;
     	double [][] dls = ref_Clt.getDLS();
     	if (dls==null) {
     		return null;
     	}
     	double [][] ds = new double [][] {dls[use_lma?1:0].clone(), dls[2]};
		final double []  ref_disparity = ds[0];
		final double []  ref_strength =  ds[1];
		final double inf_disp_ref = ref_Clt.getDispInfinityRef();
		
		if (inf_disp_ref != 0) {
			if (debugLevel >-3) {
				System.out.println("getDisparityPlaneSinglePass(): applying disparity at infinity correctio, subrtacting "+inf_disp_ref);
			}
			for (int i = 0; i < ref_disparity.length; i++) {
				ref_disparity[i] -= inf_disp_ref;
			}
		}
		final int height = ref_disparity.length / width;
		final double [] xy0= {width/2, height/2};
		int num_good = 0;
		for (int i = 0; i < good_tiles.length; i++) if (good_tiles[i]){
			num_good++;
		}
		
		double [] tilts = getPlane( //  {approx2d[0][0], approx2d[0][1], approx2d[0][2], xy0[0], xy0[1]}; // tiltX, tiltY, offset
				ref_disparity,  // final double []   data,
				good_tiles,     // final boolean [] mask,
				ref_strength,   // final double []  weight,
				width,          // final int        width,
				xy0,            // final double []  xy0,
				normal_damping, // final double     normal_damping,
				ref_Clt,        // final QuadCLT    dbg_scene,
				dbg_title1);    // String dbg_title)
		if (tilts == null) {
			System.out.println("getDisparityPlaneSinglePass(): tilts_stage1= null");
			return null;
		}
		if (debugLevel >-3) {
			System.out.println("getDisparityPlaneSinglePass(): tiltX="+tilts[0]+
					" tiltY="+tilts[1]+" offset="+tilts[2]+" fraction good="+(1.0*num_good/ref_disparity.length)+" num_good="+num_good);
		}
		return new double [] {tilts[0], tilts[1], tilts[2], 1.0*num_good/ref_disparity.length};
	}
	
	
	
	
	
	
	
	public static double [] rotateRefDisparity(
			final QuadCLT     ref_Clt,
			double []         ref_disparity,
			final double [][] scene_xyzatr) {
		double [][] pXpYD_ground = OpticalFlow.transformToScenePxPyD(
 				null,                      // final Rectangle   full_woi_in,      // show larger than sensor WOI (or null) IN TILES
 				ref_disparity,             // final double []   disparity_ref,    // invalid tiles - NaN in disparity
 				scene_xyzatr,// final double [][] scene_xyzatr,     // camera center in world coordinates, camera orientation relative to world frame
 				ref_Clt);                  // final QuadCLT     scene_QuadClt)

		double [] rot_disparity =new double [ref_disparity.length];
 		Arrays.fill(rot_disparity, Double.NaN);
 		for (int i = 0; i <ref_disparity.length; i++) {
 			double [] xyd = pXpYD_ground[i];
 			if (xyd != null) {
 				rot_disparity[i] = xyd[2];
 			}
 		}
 		return rot_disparity;
	}
	
	public static boolean saveElevationsHistogram(
			final CLTParameters clt_parameters,
			final QuadCLT       ref_Clt,
			final int           debugLevel) {
		int num_bins = 1000; // for calculating limits
		String suffix = clt_parameters.imp.fgnd_hist_suffix;
		boolean save_elev_gnd = true;
		int         around =               clt_parameters.imp.fgnd_virt_around; // 2;
		double      around_sigma =         clt_parameters.imp.fgnd_virt_sigma; // 4.0;
		int         num_virtual_refines =  clt_parameters.imp.fgnd_virt_refine; // 4;
//		String      debugSuffix=           (dbg_title != null) ? (dbg_title+"_virtual") : null; //  "virtual_gnd"; // null
		double      discard_frac =         clt_parameters.imp.fgnd_hist_frac;
	    int         rslt_bins =            clt_parameters.imp.fgnd_hist_bins;

		final double inf_disp_ref = ref_Clt.getDispInfinityRef(); // TODO: change to correction for the whole sequence, not a segment (or combine ?)
     	double [][] dls = ref_Clt.getDLS();
     	if (dls==null) {
     		return false;
     	}
     	boolean use_lma = true;
		final double []  ref_disparity = dls[use_lma?1:0].clone();
		if (inf_disp_ref != 0) {
			if (debugLevel >-3) {
				System.out.println("saveElevationsHistogram(): applying disparity at infinity correctio, subrtacting "+inf_disp_ref);
			}
			for (int i = 0; i < ref_disparity.length; i++) {
				ref_disparity[i] -= inf_disp_ref;
			}
		}
		
		GroundPlane gp = ref_Clt.getGroundPlane();
		if (gp == null) {
			System.out.println("prepareTerrainRender(): scene "+ref_Clt.toString()+" GroundPlane == null.");
			return false;
		}
		double [][] to_ground_xyzatr = gp.getToGroundPlane();
		if (to_ground_xyzatr == null) {
			System.out.println("prepareTerrainRender(): scene "+ref_Clt.toString()+" GroundPlane does not have grond plane vector.");
			return false;
		}
	    double [][] ground_xyzatr = ErsCorrection.invertXYZATR(to_ground_xyzatr); // straight down from the camera, then rotated
     	double [][] virtual_camera_from_ground_xyz = ErsCorrection.invertXYZATR(new double [][] {ground_xyzatr[0],new double[3]});
     	double [][] virtual_camera_from_camera =     ErsCorrection.combineXYZATR(ground_xyzatr, virtual_camera_from_ground_xyz);
		double [][] virtual_camera_from_camera_mod = {
				{-virtual_camera_from_camera[0][0], -virtual_camera_from_camera[0][1], virtual_camera_from_camera[0][2]},
				virtual_camera_from_camera[1] };
	    
		double [] gnd_disparity = Cuas.getDisparityVirtual(
				clt_parameters,      // CLTParameters  clt_parameters,
				ref_disparity,       // double []         disparity_ref,
				virtual_camera_from_camera_mod, // ivcfs_test,       // final double []   scene_xyz,         // camera center in world (reference) coordinates
				ref_Clt,             // final QuadCLT     reference_QuadClt,
				around,              // final int         around,            // 2 search around for interpolation
				around_sigma,        // final double      sigma,
				num_virtual_refines, // final int         num_refines,
				null); // debugSuffix);        // final String      debugSuffix)

		double [][] wxyz_gnd = OpticalFlow.transformToWorldXYZ(
				gnd_disparity,  // final double []   disparity_ref, // invalid tiles - NaN in disparity
				ref_Clt,        // final QuadCLT     quadClt, // now - may be null - for testing if scene is rotated ref
				ImageDtt.THREADS_MAX);   // int               threadsMax)

		double [] gnd_z = new double [ref_disparity.length];
		Arrays.fill(gnd_z, Double.NaN);
		for (int i = 0; i <ref_disparity.length; i++) {
			double [] wxyz_g0 = wxyz_gnd[i];
			if (wxyz_g0 != null) gnd_z[i] = wxyz_g0[2];
		}
		// save just the ground level elevation image. Add properties - orientation and
		// Create ImagePlus from a single-slice array
		
		double gnd_elevation = ground_xyzatr[0][2]; // Ground plane elevation (negative, relative to the camera)
		
		ImagePlus imp_gnd_elev = ShowDoubleFloatArrays.makeArrays( // matches -results
				gnd_z, // double[] pixels,
				ref_Clt.getTilesX(), // int width,
				ref_Clt.getTilesY(), // int height,
				""); // ref_Clt.getImageName()+ELEV_GND_SUFFIX); //  = "-ELEV_GND"
		imp_gnd_elev.setProperty("GND_ELEVATION", gnd_elevation);
		// TODO: Add ground plane orientation?
		JP46_Reader_camera.encodeProperiesToInfo(imp_gnd_elev);			
		if (save_elev_gnd) {
			ref_Clt.saveImagePlusInModelDirectory(
					ELEV_GND_SUFFIX,          // String      suffix, // null - use title from the imp
					imp_gnd_elev); // ImagePlus   imp)
		}
		// now calculate the cumulative histogram
//		double avg_gnd_lev = weightedAverage(gnd_z, null); // negative, relative
		double [] gnd_elev = gnd_z.clone();
		for (int i = 0; i < gnd_elev.length; i++) {
			gnd_elev[i] -= gnd_elevation;
		}
		double [] safe_min_max = getMinMax(
				gnd_elev,
				gnd_elevation,   // safe_min, twice ground level
				-gnd_elevation); // positive, camera level relative to the ground

		double [] hist1000 = getHistogram1d(
				gnd_elev,             // double [] data, // may have NaNs
				null,              // double [] weights,
				safe_min_max[0],   // double    hist_low,
				safe_min_max[1],   // double    hist_high,
				num_bins,          // int       num_bins,
				debugLevel);       // int       debugLevel)
		double [] fractions = {discard_frac, 1.0-discard_frac};
		double [] percentiles = getPercentiles(
				hist1000,          // double [] hist,
				fractions);        // double [] fractions)
		double low_lim =  safe_min_max[0] + percentiles[0] * (safe_min_max[1]-safe_min_max[0]);
		double high_lim = safe_min_max[0] + percentiles[1] * (safe_min_max[1]-safe_min_max[0]);
		
		double [] hist_rslt = getHistogram1d(
				gnd_elev,    // double [] data, // may have NaNs
				null,        // double [] weights,
				low_lim,     // double    hist_low,
				high_lim,    // double    hist_high,
				rslt_bins,   // int       num_bins,
				debugLevel); // int       debugLevel)
		double [] hist_cumul = hist_rslt.clone();
		for (int i = 1; i < hist_rslt.length; i++) {
			hist_cumul[i] += hist_cumul[i - 1]; 
		}
		double hist_sum = hist_cumul[hist_cumul.length-1];
		for (int i = 0; i < hist_cumul.length; i++) {
			hist_cumul[i] /= hist_sum;
		}
		double step = (high_lim - low_lim)/rslt_bins;
		StringBuffer sb = new StringBuffer();
		sb.append("AGL\tELEVATION\tFRACTION_LOWER\n");
		for (int i = 0; i < hist_cumul.length; i++) {
			if (i==0) {
				sb.append(-gnd_elevation);
			}
			double v = low_lim + (i+1)*step;   
			sb.append("\t"+v+"\t"+hist_cumul[i]+"\n");
		}
	    Path hist_path = Paths.get(ref_Clt.getX3dDirectory(true)).resolve(ref_Clt.getImageName()+suffix);
		CalibrationFileManagement.saveStringToFile (
				hist_path.toString(),
				sb.toString());
		return true;
	}
	
	public static double [][] prepareTerrainRender(
			final CLTParameters clt_parameters,
			final QuadCLT       ref_Clt,
			final boolean       tilted_plane, // if true, use tilted ground plane. If false - use horizontal plane and apply rotation
			final double        offset,
			final double        test_bottom,
			final int           debugLevel) {
		
		GroundPlane gp = ref_Clt.getGroundPlane();
		if (gp == null) {
			System.out.println("prepareTerrainRender(): scene "+ref_Clt.toString()+" GroundPlane == null.");
			return null;
		}
		double [][] to_ground_xyzatr = gp.getToGroundPlane();
		if (to_ground_xyzatr == null) {
			System.out.println("prepareTerrainRender(): scene "+ref_Clt.toString()+" GroundPlane does not have grond plane vector.");
			return null;
		}
	    double [][] ground_xyzatr = ErsCorrection.invertXYZATR(to_ground_xyzatr); // straight down from the camera, then rotated
		double altitude = ground_xyzatr[0][2] + offset; // offset to the ground level (negative)
		double true_disparity = ref_Clt.getGeometryCorrection().getDisparityFromZ(-altitude);
		double corrected_disparity = true_disparity+ref_Clt.getDispInfinityRef();
		int tilesX = ref_Clt.getTilesX();
		int tilesY = ref_Clt.getTilesY();
		double [] terrain_plane = new double [tilesX*tilesY];
		if (tilted_plane) {
			int [] xy0 = {tilesX/2, tilesY/2};
			double inf_disparity = ref_Clt.getDispInfinityRef();
			double [] disparity_tilts = gp.getDisparityTilts();
			if ((test_bottom > 0) && (test_bottom < 1)) {
				int y1 = (int) ((terrain_plane.length/tilesX) * test_bottom);
				if (disparity_tilts != null) {
					for (int i = 0; i < terrain_plane.length; i++) {
						int x = i % tilesX;  
						int y = i / tilesX;
						if (y > y1) {
							y = y1;
						}
						double dx = x-xy0[0];
						double dy = y-xy0[1];
						terrain_plane[i] = disparity_tilts[0] * dx +  disparity_tilts[1] * dy + disparity_tilts[2] + inf_disparity;
					}
				}
			} else {
				if (disparity_tilts != null) {
					for (int i = 0; i < terrain_plane.length; i++) {
						int x = i % tilesX;  
						int y = i / tilesX;
						double dx = x-xy0[0];
						double dy = y-xy0[1];
						terrain_plane[i] = disparity_tilts[0] * dx +  disparity_tilts[1] * dy + disparity_tilts[2] + inf_disparity;
					}
				}
			}
		} else {
			Arrays.fill(terrain_plane, corrected_disparity); // +0.3); // 260); // 0.256);
		}
		
		double [][] virt_camera = virtCameraFromToGround( // virtual to reference
				to_ground_xyzatr);  // double [][] to_ground_xyzatr
		
		double [][] stereo_xyzatr = ErsCorrection.invertXYZATR(virt_camera);

		ref_Clt.setTerrain(
				clt_parameters,  // CLTParameters clt_parameters,
				terrain_plane,   // double []     terrain,
	    		debugLevel);     // int           debugLevel)
		return stereo_xyzatr;				
	}
	
	public static double [][] virtCameraFromToGround(
			double [][] to_ground_xyzatr
			){
	    double [][] ground_xyzatr = ErsCorrection.invertXYZATR(to_ground_xyzatr); // straight down from the camera, then rotated
     	double [][] virtual_camera_from_ground_xyz = ErsCorrection.invertXYZATR(new double [][] {ground_xyzatr[0],new double[3]});
     	double [][] virtual_camera_from_camera =     ErsCorrection.combineXYZATR(ground_xyzatr, virtual_camera_from_ground_xyz);
      	virtual_camera_from_camera[1] = to_ground_xyzatr[1]; // *******????????
     	return virtual_camera_from_camera;
	}

	public static double [] getPlaneDualPass( // returns tiltX, tiltY, disp_center, frac_good
			final CLTParameters clt_parameters,
			final double []  data,
			final double []  weights,
			final int        width,
			final boolean [] good_tiles, // null or boolean[data.length] // should all be false
			final String     dbg_title,
			final QuadCLT    dbg_scene,
			final int        debugLevel) {
		
		final double   gnd_percent_low = clt_parameters.imp.fgnd_percent_low; // 0.01; // Discard lowest outliers. Percentile to remove lowest elevation outliers during initial terrain filtering
		final double   gnd_percent_high= clt_parameters.imp.fgnd_percent_high; // 0.2;  // Discard highest outliers. Percentile to remove highest elevation outliers during initial terrain filtering.
		final double   gnd_max_high_over=clt_parameters.imp.fgnd_max_high_over; // 0.1;  // Absolute disparity over lowest. Absolute disparity over lowest. Minimal of this value and the one determined by the highest outliers percentile will be applied. // pix = make dependent on average disparity?
//		final int      min_good1 =       clt_parameters.imp.fgnd_min_good1; //10;    // Minimal good tiles in stage 1. Minimal number of remaining tiles after the first stage filtering.
		final double   max_abs_diff =    clt_parameters.imp.fgnd_max_abs_diff; // 0.03; // Absolute disparity from plane. Maximal absolute disparity difference from the stage 1 - found plane (stage2 mask).
		final double   max_rel_diff =    clt_parameters.imp.fgnd_max_rel_diff; // 0.03; // Relative disparity from plane. Maximal relative disparity difference from the stage 1 - found plane (stage2 mask).
		final double   normal_damping =  clt_parameters.imp.fgnd_normal_damping; // 0.001; // Damping parameter for planes. Pull planes to horizontal when there is not enough data for tilts.
		
		
		
		int num_bins = 1000;
		String dbg_title1 = (dbg_title != null) ? (dbg_title + "-stage1") : null;
		String dbg_title2 = (dbg_title != null) ? (dbg_title + "-stage2") : null;
		final int height = data.length / width;
		final double [] xy0= {width/2, height/2};
		double avg_val = weightedAverage (
				data,   // double [] data, // may have NaNs
				weights); // double [] weights)
		double hist_low = 0.0;
		double hist_high = 1.5 * avg_val;
		double [] hist = getHistogram1d(
				data,         // double [] data, // may have NaNs
				weights,      // double [] weights,
				hist_low,     // double    hist_low,
				hist_high,    // double    hist_high,
				num_bins,     // int       num_bins,
				debugLevel);  // int       debugLevel)
		double [] fractions = {gnd_percent_low, gnd_percent_high};
		double [] percentiles = getPercentiles(
				hist,          // double [] hist,
				fractions);    // double [] fractions)
		double low_lim =  hist_low + percentiles[0] * (hist_high-hist_low);
		double high_lim = hist_low + percentiles[1] * (hist_high-hist_low);
		if (high_lim > (low_lim + gnd_max_high_over)) {
			high_lim = low_lim + gnd_max_high_over;
		}
		boolean [] mask = new boolean [data.length];
		int num_good1=0;
		for (int i = 0; i < mask.length; i++) {
			if ((data[i] >= low_lim) && (data[i] <= high_lim)) {
				mask[i] = true;
				num_good1++;
			}
		}
		if (debugLevel >-3) {
			System.out.println("getPlaneDualPass(): num_good1="+num_good1+
					" low_lim="+low_lim+" high_lim="+high_lim+" fractions=["+fractions[0]+","+fractions[1]+"]");
		}

		
		double [] tilts_stage1 = getPlane( //  {approx2d[0][0], approx2d[0][1], approx2d[0][2], xy0[0], xy0[1]}; // tiltX, tiltY, offset
				data,           // final double []   data,
				mask,           // final boolean [] mask,
				weights,        // final double []  weight,
				width,          // final int        width,
				xy0,            // final double []  xy0,
				normal_damping, // final double     normal_damping,
				dbg_scene,      // final QuadCLT    dbg_scene,
				dbg_title1);    // String dbg_title)
		if (tilts_stage1 == null) {
			System.out.println("getPlaneDualPass(): tilts_stage1= null");
			return null;
		}
		if (debugLevel >-3) {
			System.out.println("getPlaneDualPass() stage1: tiltX="+tilts_stage1[0]+
					" tiltY="+tilts_stage1[1]+" offset="+tilts_stage1[2]+" fraction good1="+(1.0*num_good1/data.length)+" num_good1="+num_good1);
		}
		double [] data_tilted = new double[data.length];
		boolean [] mask2 = new boolean [data.length];
		int num_good2 = 0;
		double max_diff = Math.min(max_abs_diff, max_rel_diff * low_lim);
		for (int i = 0; i < data.length; i++) {
			int x = i % width;  
			int y = i / width;
			double dx = x-xy0[0];
			double dy = y-xy0[1];
			double plane = tilts_stage1[0] * dx +  tilts_stage1[1] * dy + tilts_stage1[2];
			data_tilted[i] = data[i] - plane;
			if (Math.abs(data_tilted[i]) <=  max_diff) {
				mask2[i] = true;
				num_good2++;
			}
		}
		if (debugLevel >-3) {
			System.out.println("getPlaneDualPass(): num_good2="+num_good2+ " max_diff="+max_diff);
		}

		
		double [] tilts_stage2 = getPlane( //  {approx2d[0][0], approx2d[0][1], approx2d[0][2], xy0[0], xy0[1]}; // tiltX, tiltY, offset
				data_tilted,    // final double []   data,
				mask2,          // final boolean [] mask,
				weights,        // final double []  weight,
				width,          // final int        width,
				xy0,            // final double []  xy0,
				normal_damping, // final double     normal_damping,
				dbg_scene,      // final QuadCLT    dbg_scene,
				dbg_title2);     // String dbg_title)
		if (tilts_stage2 == null) {
			System.out.println("getPlaneDualPass(): tilts_stage2= null, using results of stage1");
			double [] rslt_tilts = {tilts_stage1[0], tilts_stage1[1], tilts_stage1[2], 1.0 * num_good2/data.length};
			return rslt_tilts;
		}

		if (debugLevel >-3) {
			System.out.println("getPlaneDualPass() stage2: tiltX="+tilts_stage2[0]+
					" tiltY="+tilts_stage2[1]+" offset="+tilts_stage2[2]+" fraction good2="+(1.0*num_good2/data.length)+" num_good2="+num_good2);
		}

		
		double [] rslt_tilts = {tilts_stage1[0]+tilts_stage2[0],tilts_stage1[1]+tilts_stage2[1],tilts_stage1[2]+tilts_stage2[2], 0};
		int num_good =0;
		if (good_tiles != null) {
			Arrays.fill(good_tiles, false);
		}
		for (int i = 0; i < data.length; i++) {
			int x = i % width;  
			int y = i / width;
			double dx = x-xy0[0];
			double dy = y-xy0[1];
			double plane = rslt_tilts[0] * dx +  rslt_tilts[1] * dy + rslt_tilts[2];
			double diff = data[i] - plane;
			if (Math.abs(diff) <=  max_diff) {
				num_good++;
				if (good_tiles != null) {
					good_tiles[i] = true;
				}
			}

		}
		rslt_tilts[3] = 1.0*num_good/width/height;
		if (debugLevel >-3) {
			System.out.println("getPlaneDualPass(): tiltX="+rslt_tilts[0]+
					" tiltY="+rslt_tilts[1]+" offset="+rslt_tilts[2]+" fraction good="+rslt_tilts[3]+" num_good="+num_good);
		}
		if ((dbg_title != null) && (dbg_scene != null)) {
			String [] dbg_titles={"disparity","tilted","masked","strength"};
			double [][] dbg_img = new double [dbg_titles.length][width*height];
			for (int i = 0; i < data.length; i++) {
				int x = i % width;  
				int y = i / width;
				double dx = x-xy0[0];
				double dy = y-xy0[1];
				double plane = rslt_tilts[0] * dx +  rslt_tilts[1] * dy; //  + rslt_tilts[2];
				dbg_img[0][i]= data[i];
				dbg_img[1][i]= data[i]-plane;
				dbg_img[2][i]= ((good_tiles != null) && good_tiles[i]) ? (data[i]-plane) : Double.NaN;
				if (weights != null) {
					dbg_img[3][i]= weights[i];
				}
			}
			dbg_scene.saveDoubleArrayInModelDirectory(
					dbg_title+"-result", // String      suffix,
					dbg_titles,          // String []   labels, // or null
					dbg_img,             // double [][] data,
					width,               // int            width, // int tilesX,
					data.length/width);  // int            height, // int tilesY,

		}
		return rslt_tilts;
	}
	
	
	// copied from OrthoMap.java
	/**
	 * 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]).
	 */
	public static double [] getPlane(
			final double []  data,
			final boolean [] mask,
			final double []  weight,
			final int        width,
			final double []  xy0,
			final double     normal_damping
			) {
		return getPlane(
				   data, // final double []   data,
				mask,  // final boolean [] mask,
				weight, //final double []  weight,
				width, // final int        width,
				xy0, // final double []  xy0,
				normal_damping,
				null,  // final QuadCLT    dbg_scene,
				null); // String dbg_title)
	}
	public static double [] getPlane(
			final double []   data,
			final boolean [] mask,
			final double []  weight,
			final int        width,
			final double []  xy0,
			final double     normal_damping,
			final QuadCLT    dbg_scene,
			String dbg_title) {
     	double [] damping = Double.isNaN(normal_damping) ? null : (new double [] {normal_damping, normal_damping});
		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 == null) || 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
				damping,    // damping,    // double [] damping, null OK
				-1);        // debug level
		if (approx2d != null) {
			if ((dbg_title != null) && (dbg_scene != null)) {
				double [] plane=new double[data.length];
				double [] diff=new double[data.length];
				double [] dweight = (weight != null) ? weight: new double[data.length];
				double [] dmask =   new double[data.length];
				double [] masked = new double[data.length];
				Arrays.fill(masked, Double.NaN);
				for (int nPix = 0; nPix < plane.length; nPix++) {
					int x = nPix % width;  
					int y = nPix / width;
					double dx = x-xy0[0];
					double dy = y-xy0[1];
					plane[nPix] = approx2d[0][2]+approx2d[0][0]*dx+approx2d[0][1]*dy;
					diff[nPix] = data[nPix]-plane[nPix];
					dmask[nPix] = ((mask == null) || Double.isNaN(data[nPix]))? Double.NaN : (mask[nPix]?1:0);
					if ((mask == null) || mask[nPix]) {
						masked[nPix] = diff[nPix];
					}
				}
				
				dbg_scene.saveDoubleArrayInModelDirectory(
						dbg_title,                                                       // String      suffix,
						new String[] {"data","approx","diff","masked","weight","mask"},  // String []   labels, // or null
						new double[][] {data,plane,diff,masked,dweight,dmask},           // double [][] data,
						width,                                                           // int            width, // int tilesX,
						data.length/width);                                              // int            height, // int tilesY,
			}
			
			return new double[] {approx2d[0][0], approx2d[0][1], approx2d[0][2], xy0[0], xy0[1]}; // tiltX, tiltY, offset
		}
		return null;
	}
	
	/**
	 * Split frame area into 50% macro tiles, and calculate tilts, offset, and confidence
	 * derived from the eigenvalues
	 * @param wxyz array of X, Y, Z of the elevation surface relative to the camera reference scene. May have nulls
	 * @param mask corresponding tile mask
	 * @param weight array of tile weights   
	 * @param tilesX elevation map width (in tiles). Now 80
	 * @param mtile_size width of the square macrotiles in tiles (also 16?) 
 	 * @param normal_damping regularization parameter to make tilts 0 if there is not enough data to calculate them
	 * @param dbg_title if not null, use it to generate debug images
	 * @param debugLevel debug level, now is tested if (debugLevel > -3) 
	 * @return array of per-macrotile: offset at frame center, tiltX, tiltY, product of eigenvalues, both eigenvalues (starting with smaller)
	 */
	public static double [][] getMacroPlanes(
			final double [][] wxyz,
			final boolean []  mask,
			final double []   weight,
			final boolean     eig_weight, // use weights when calculating eiget values (false - only mask)
			final int         tilesX,
			final int         mtile_size,
			final double      normal_damping,
			final String      dbg_title,
			final int         debugLevel) {
		final int tilesY = wxyz.length/tilesX;
		final int mtile_half = mtile_size/2;
		final int mtilesX = (tilesX-mtile_size)/mtile_half + 1;
		final int mtilesY = (tilesY-mtile_size)/mtile_half + 1;
		final double [][] mtiles = new double [mtilesX*mtilesY][];
     	final double [] damping = Double.isNaN(normal_damping) ? null : (new double [] {normal_damping, normal_damping});
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
		// Calculate eigenvalues for all square is full of weight = 1.0;
		final int eig_full = ((mtile_half-1)*mtile_half*(2*mtile_half-1)/3 + mtile_half*(mtile_half-1) +mtile_half/2)*2*mtile_half;
		final double scale_eig = 1.0/eig_full;
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					final double [][][] mdata = new double [mtile_size * mtile_size][][];
					for (int mTile = ai.getAndIncrement(); mTile < mtiles.length; mTile = ai.getAndIncrement()){
						int ntile = ((mTile / mtilesX) * tilesX + (mTile % mtilesX)) * mtile_half; // top-left
						int local_xc = ((mTile % mtilesX) + 1) * mtile_half; 
						int local_yc = ((mTile / mtilesX) + 1) * mtile_half;
						int ntile_center = local_xc + tilesX*local_yc; // wxyz may be null, will not use local center

						int mindx = 0;
						// for centroid and eigenvalues
						double x0 = mtile_half, y0 = mtile_half;
						double s0 = 0, sx=0,sy = 0, sx2 = 0, sy2=0, sxy = 0;
						for (int my = 0; my < mtile_size; my++) {
							double y = my - y0; 
							for (int mx = 0; mx < mtile_size; mx++) {
								if ((wxyz[ntile] != null) && ((mask == null) || mask[ntile])) {
									double x = mx - x0; 
									double w = (weight == null)? 1.0 : weight[ntile];
									mdata[mindx] = new double[3][];
									mdata[mindx][0] = new double [2];
									mdata[mindx][0][0] = wxyz[ntile][0]; // from global center
									mdata[mindx][0][1] = wxyz[ntile][1];
									mdata[mindx][1] = new double [1];
									mdata[mindx][1][0] =   wxyz[ntile][2]; 
									mdata[mindx][2] = new double [1];
									mdata[mindx][2][0] =  w;
									// for eigenvalues
									if (!eig_weight) {
										w = 1.0;
									}
									s0 += w;
									sx += w * x;
									sy += w * y;
									sx2 += w * x * x;
									sy2 += w * y * y;
									sxy += w * x * y;
									mindx++;
								}
								ntile++;	
							}
							ntile += (tilesX-mtile_size);
						}

						if (mindx > 0) { // OK to have even a single tile
							Arrays.fill(mdata, mindx, mdata.length, null); 
							double[][] approx2d = (new PolynomialApproximation()).quadraticApproximation(
									mdata,
									true,       // boolean forceLinear,  // use linear approximation
									damping,    // damping,    // double [] damping, null OK
									-1);        // debug level
							if (approx2d == null) {
								continue;
							}
							//									z_tilts= new double [] { approx2d[0][2], approx2d[0][0], approx2d[0][1]};
							//						int ntile = ((mTile / mtilesX) * tilesX + (mTile % mtilesX)) * mtile_half;
							int dx = tilesX/2 - local_xc; // center relative to this
							int dy = tilesY/2 - local_yc;

							mtiles[mTile] = new double[6]; // center offset, tiltx, tilty, eigenvalues product, eigenvalue0, eigenvalue1
							// can not shift as do not know wx, wy at local center
							mtiles[mTile][0] =  approx2d[0][2]; //  + dx *  approx2d[0][0] + dy * approx2d[0][1]; // shift to center
							mtiles[mTile][1] =  approx2d[0][0];
							mtiles[mTile][2] =  approx2d[0][1];
							mtiles[mTile][3] =  Double.NaN;
							mtiles[mTile][4] =  Double.NaN;
							mtiles[mTile][5] =  Double.NaN;
							
							// calculate eigenvalues of weight*mask? Use both or a product as a measure of confidence
							x0 += sx / s0; // will these be used?
							y0 += sy / s0;
							//https://users.cs.utah.edu/~tch/CS4640/resources/A%20geometric%20interpretation%20of%20the%20covariance%20matrix.pdf
							double cxx = sx2 - sx * sx / s0, cyy= sy2 - sy * sy / s0, cxy = sxy - sx * sy / s0;
							if ((sx2 != 0) && (sy2 != 0) && (cxx != 0) && (cyy != 0)){
								double [][] acovar =  {{cxx, cxy},{cxy,cyy}};								
								double [] eig = Correlation2d.getEigen2x2(acovar); // return 4-element array: first eigenvector x, y, lambda0, lambda1 (lambda0 <= lampda1)
								if (eig != null) {
									double eig0= Math.sqrt(scale_eig*eig[2]);
									double eig1= Math.sqrt(scale_eig*eig[3]);
									mtiles[mTile][3] =  Math.sqrt(eig0 * eig1);
									mtiles[mTile][4] =  eig0; // eig[2] < eig[3]; 
									mtiles[mTile][5] =  eig1;
								}
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return mtiles;
	}
	
	
	
	public static double [][] getPlane( // from the camera coordinates to in-plane coordinates
			final double [][] wxyz,
			final boolean     use_parallel_proj, // for parallel xyz is 0, otherwise - point on the ground under the camera
			final boolean []  mask,
			final double []   weight,
			final int         width,
			final double      normal_damping,
			final String      dbg_title,
			final int         debugLevel) {
		double [] z_tilts = null;
     	double [] damping = Double.isNaN(normal_damping) ? null : (new double [] {normal_damping, normal_damping});
		final double [][][] mdatai = new double [wxyz.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 < wxyz.length; nPix = ai.getAndIncrement()) if ((mask == null) || mask[nPix]){
						if (wxyz[nPix] != null) {
							int mindx = anum_good.getAndIncrement();
							mdatai[mindx] = new double[3][];
							mdatai[mindx][0] = new double [2];
							mdatai[mindx][0][0] = wxyz[nPix][0];
							mdatai[mindx][0][1] = wxyz[nPix][1];
							mdatai[mindx][1] = new double [1];
							mdatai[mindx][1][0] =   wxyz[nPix][2]; 
							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
				damping,    // damping,    // double [] damping, null OK
				-1);        // debug level
		if (approx2d == null) {
			System.out.println("getPlane() approx2d== null");
			return null;
		}
		z_tilts= new double [] { approx2d[0][2], approx2d[0][0], approx2d[0][1]};
		if (debugLevel > -3) {
			System.out.println("Found ground plane: level="+z_tilts[0]+", tiltX="+z_tilts[1]+", tiltY="+z_tilts[2]);
		}
///     	double [][] ground_xyzatr =  new double [][] {{0, 0, use_parallel_proj?0:z_tilts[0]},{Math.asin(z_tilts[1]), -Math.asin(z_tilts[2]), 0.0}};
///     	double [][] ground_xyzatr00 =  new double [][] {{0, 0, use_parallel_proj?0:z_tilts[0]},{-Math.asin(z_tilts[1]), Math.asin(z_tilts[2]), 0.0}};
     	// or opposite order?
     	boolean invert_order = false;
		if (debugLevel > -3) {
			System.out.println("Using "+(invert_order?"inverted":"direct")+" azimuth/tilt order when convertingtiltX/tiltY to angles.");
		}
     	
     	double tl = 0, az=0;
     	if (invert_order) {
     		tl = -Math.atan(-z_tilts[2]/Math.sqrt(1+z_tilts[1]*z_tilts[1]));
     		az = -Math.atan(z_tilts[1]);
     	} else {
     		az = Math.atan(-z_tilts[1]/Math.sqrt(1+z_tilts[2]*z_tilts[2]));
     		tl = Math.atan(z_tilts[2]);
     	}
 		double [][] ground_xyzatr =  new double [][] {{0, 0, use_parallel_proj?0:z_tilts[0]},{az, tl, 0.0}};
     	
     	// It is approximate for small angles. OK for now    		
     	double [][] to_ground_xyzatr = ErsCorrection.invertXYZATR(ground_xyzatr);
     	return to_ground_xyzatr; // from the camera coordinates to in-plane coordinates
	}
	
	static double [] getMinMax(double [] data) {
		double [] min_max = {Double.NaN,Double.NaN};
		for (int i = 0; i< data.length; i++) {
			double d = data[i];
			if (!Double.isNaN(d)) {
				if (!(d >= min_max[0])) min_max[0]=d; 
				if (!(d <= min_max[1])) min_max[1]=d; 
			}
		}
		return min_max;
	}
	static double [] getMinMax(
			double [] data,
			double safe_min,
			double safe_max) {
		double [] min_max = {Double.NaN,Double.NaN};
		for (int i = 0; i< data.length; i++) {
			double d = data[i];
			if (!Double.isNaN(d) && (d >= safe_min) && (d <= safe_max)) {
				if (!(d >= min_max[0])) min_max[0]=d; 
				if (!(d <= min_max[1])) min_max[1]=d; 
			}
		}
		return min_max;
	}
	
	static double [] getHistogram1d (
			double [] data, // may have NaNs
			double [] weights,
			double    hist_low,
			double    hist_high,
			int       num_bins,
			int       debugLevel) {
		if (weights == null) {
			weights = new double [data.length];
			Arrays.fill(weights, 1.0);
		}
     	double k = num_bins / (hist_high - hist_low);
     	double [] hist = new double [num_bins];
     	for (int i = 0; i < data.length; i++) if (!Double.isNaN(data[i])){
     		double d = data[i];
     		double w = weights[i];
     		int bin = (int) (k * (d - hist_low));
     		if ((bin >= 0) && (bin < num_bins)) {
     			hist[bin] += w;
     		}
     	}
		return hist;
	}
	
	static double [] getPercentiles(
			double [] hist,
			double [] fractions) {
		double [] percentiles = new double [fractions.length];
		double [] hist_cumul = new double [hist.length+1];
		for (int i = 0; i < hist.length; i++) {
			hist_cumul[i+1] = hist_cumul[i] + hist[i];
		}
		double s = hist_cumul[hist_cumul.length-1];
		for (int nfrac = 0; nfrac < fractions.length; nfrac++) {
			double f = fractions[nfrac];
			double p = 0;
			double v = f * s;
			int indx = 0;
			for (; indx < hist_cumul.length; indx++) {
				if (hist_cumul[indx] > v) {
					break;
				}
			}
			if (indx == 0) {
				p = 0;
			} else if (indx >= hist_cumul.length) {
				p = 1;
			} else {
				double frac = (v - hist_cumul[indx - 1])/(hist_cumul[indx] - hist_cumul[indx - 1]);
				p= (indx - 1 + frac)/hist.length; 
			}
			percentiles[nfrac] = p;
		}
		
		return percentiles;
	}
	

	static double weightedAverage (
			double [] data, // may have NaNs
			double [] weights) {
		double sw = 0, swd = 0;
     	for (int i = 0; i < data.length; i++) if (!Double.isNaN(data[i])){
     		double d = data[i];
     		double w = (weights != null) ? weights[i] : 1.0;
     		swd += d * w;
     		sw += w;
     	}
		return swd/sw;
	}
	
}
