BiScan.java

package com.elphel.imagej.tileprocessor;
/**
 ** BiCamScan - class to represent multiple bi-quad camera measurements
 **
 ** Copyright (C) 2018 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  BiCamScan.java is free software: you can redistribute it and/or modify
 **  it under the terms of the GNU General Public License as published by
 **  the Free Software Foundation, either version 3 of the License, or
 **  (at your option) any later version.
 **
 **  This program is distributed in the hope that it will be useful,
 **  but WITHOUT ANY WARRANTY; without even the implied warranty of
 **  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 **  GNU General Public License for more details.
 **
 **  You should have received a copy of the GNU General Public License
 **  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 ** -----------------------------------------------------------------------------**
 **
 */
import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;
import java.util.concurrent.atomic.AtomicInteger;

import com.elphel.imagej.common.PolynomialApproximation;
import com.elphel.imagej.common.ShowDoubleFloatArrays;

public class BiScan {
	final static double THRESHOLD_LIN = 1.0E-20;  // threshold ratio of matrix determinant to norm for linear approximation (det too low - fail)
	final static double THRESHOLD_QUAD = 1.0E-30; // threshold ratio of matrix determinant to norm for quadratic approximation (det too low - fail)
	final static int    BISCAN_ANY =       -1;
	final static int    BISCAN_SINGLECORR = 0;
	final static int    BISCAN_AVGCORR =    1; // combined with low-texture averaging correlation
	final static int    BISCAN_POLE =       2; // combined with low-texture averaging correlation

	double []  disparity_measured;
	double []  target_disparity;
	double []  strength_measured;
	boolean [] strong_trusted; // sufficient strength without neighbors
	boolean [] trusted;
	boolean [] cond_trusted;
	boolean [] disabled_measurement; // should disable source, not this!
	int     [] src_index;       // index of the source scan which measured data is used here (applies to disparity_measured, strength_measured, disabled_measurement
	int        list_index = -1;
	int        scan_type = -1;
	BiCamDSI biCamDSI;
//	public BiScan(BiCamDSI biCamDSI) {
//		this.biCamDSI = biCamDSI;
//		int num_tiles = biCamDSI.tnImage.getSizeX()*biCamDSI.tnImage.getSizeY();
//		disparity= new double[num_tiles];
//		strength=  new double[num_tiles];
//		trusted=   new boolean[num_tiles];
//	}
	public BiScan(
			BiCamDSI   biCamDSI,
			int        indx,
			double []  disparity,
			double []  strength,
			boolean [] trusted,
			boolean [] disabled,
			int scan_type) {
		this.biCamDSI = biCamDSI;
		this.list_index = indx;
		this.scan_type = scan_type;
		int num_tiles = biCamDSI.tnImage.getSizeX()*biCamDSI.tnImage.getSizeY();
		if (disparity == null) {
			disparity= new double[num_tiles];
		} else {
			this.disparity_measured = disparity.clone();
			if (strength == null)  {
				strength=  new double[num_tiles];
				for (int nTile = 0; nTile < num_tiles; nTile++) strength[nTile] = Double.isNaN(disparity[nTile])?0.0:1.0;
			} else {
				this.strength_measured = strength.clone();
				for (int nTile = 0; nTile < num_tiles; nTile++) {
					if (Double.isNaN(disparity[nTile])) this.strength_measured [nTile] = 0.0;
					if (strength[nTile] == 0.0)         this.disparity_measured[nTile] = Double.NaN;
				}
			}
		}
		if (trusted == null)   trusted=   new boolean[num_tiles];
		if (disabled == null)  disabled=  new boolean[num_tiles];
		this.trusted = trusted;
		this.disabled_measurement = disabled;
		this.strong_trusted = new boolean[num_tiles];
		this.cond_trusted =   new boolean[num_tiles];
		src_index = new int[num_tiles];
		// set new measurement index to this, other to -1
		for (int i = 0; i < num_tiles; i++) {
			src_index[i] = (strength[i] > 0.0)? list_index:-1;
		}
	}

	public double []  getDisparityMeasured()   { return this.disparity_measured;} // FIXME!
	public double []  getStrengthMeasured()    { return this.strength_measured;} // FIXME
	public boolean [] getTrusted()             { return this.trusted;}
	public boolean [] getDisabledMeasurement() { return this.disabled_measurement;}

	public void disableTile (int nTile) {
		trusted[nTile] =        false;
		strong_trusted[nTile] = false;
		cond_trusted[nTile] =   false;
		//    	disabled[nTile] =       true;
		//    	if ((src_index[nTile] >= 0) && (src_index[nTile] != list_index)) {
		if (src_index[nTile] >= 0) {
			biCamDSI.getBiScan(src_index[nTile]).disabled_measurement[nTile] = true; // false; // may be source tile or this tile
		}
	}
    /**
     * Get disparity and strength from the scan, mask by boolean attributes
     * @param only_strong keep only trusted strong tiles
     * @param only_trusted keep any trusted tiles
     * @param only_enabled keep all but disabled tiles
     * @return array of two arrays {disparity, strength}
     */

    public double [][] getDisparityStrength( // FIXME
    		final boolean only_strong,
    		final boolean only_trusted,
    		final boolean only_enabled){
    	final int num_tiles = disparity_measured.length;
    	final double [][] ds = {disparity_measured.clone(), strength_measured.clone()}; // just to start with
		final Thread[] threads = ImageDtt.newThreadArray(biCamDSI.threadsMax);
		final AtomicInteger ai = new AtomicInteger(0);
		final BiScan this_scan = this;
		// find definitely trusted and conditionally trusted tiles
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				@Override
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) {
						if ((only_strong && !strong_trusted[nTile]) ||
								(only_trusted && !trusted[nTile])) {
							ds[0][nTile] = Double.NaN;
							ds[1][nTile] = 0.0;
						} else { //if (src_index[nTile] != list_index){ // only one level of indirection?
							int src = src_index[nTile]; // same tile or different
							BiScan scan = this_scan;
							if (src < 0)  {
								src = list_index;
							} else {
								scan = biCamDSI.biScans.get(src);
							}
							boolean dsbl = scan.disabled_measurement[nTile];
							if (dsbl && only_enabled) { // src <0 only for the first scan where no data is available
								ds[0][nTile] = Double.NaN;
								ds[1][nTile] = 0.0;
							} else {
								ds[0][nTile] = scan.disparity_measured[nTile];
								ds[1][nTile] = scan.strength_measured[nTile];
							} // if (dsbl && only_enabled) -- else
						}
					}
				}
			};
		}
		ImageDtt.startAndJoin(threads);
    	return ds;
    }

    class DSIndex{
    	double disparity;
    	double strength;
    	int scan_index;
    	DSIndex (
    	    	double disparity,
    	    	double strength,
    	    	int scan_index)
    	{
        	this.disparity = disparity;
        	this.strength = strength;
        	this.scan_index = scan_index;

    	}
    }

    ArrayList<DSIndex> getTileDSList(
    		int           nTile,
    		final boolean only_strong,
    		final boolean only_trusted,
    		final boolean only_enabled)
    {
    	ArrayList<DSIndex> ds_list = new ArrayList<DSIndex>();
		for (int indx = list_index; indx >= 0; indx--) { // include the latest measurement
			BiScan scan = biCamDSI.getBiScan(indx);
			if (only_enabled && scan.disabled_measurement[nTile]) { //  || (scan.src_index[nTile] != indx)){ // skip all but enabled
				continue;
			}
			if (only_strong && !scan.strong_trusted[nTile]) {
				continue;
			}
			if	(only_trusted && !scan.trusted[nTile]) {
				continue;
			}
			if ((scan.strength_measured[nTile] > 0.0) && !Double.isNaN(scan.disparity_measured[nTile])) {
				ds_list.add(new DSIndex(scan.disparity_measured[nTile],scan.strength_measured[nTile],indx));
			}

		}
		Collections.sort(ds_list, new Comparator<DSIndex>() {
			@Override
			public int compare(DSIndex lhs, DSIndex rhs) {
				// -1 - less than, 1 - greater than, 0 - equal, all inverted for descending disparity
				return lhs.disparity > rhs.disparity ? -1 : (lhs.disparity < rhs.disparity ) ? 1 : 0;
			}
		});
    	return ds_list;


    }



    // trusted should be set, copied and replaced as needed
    public double [][] getFilteredDisparityStrength( // FIXME
			final boolean [] area_of_interest,
			final double [][] disparityStrength,
			final double     min_disparity,    // keep original disparity far tiles
		    final double     trusted_strength, // trusted correlation strength
			final double     strength_rfloor,   // strength floor - relative to trusted
		    final boolean    discard_unreliable,// replace v
		    final boolean    discard_weak,      // consider weak trusted tiles (not promoted to trusted) as empty
		    final boolean    discard_strong,    // suggest new disparitieas even for strong tiles
			final double     strength_pow,      // raise strength-floor to this power
			final double []  smpl_radius_array, // space-variant radius
			final int        smpl_radius,
			final int        smpl_num,   //         = 3;      // Number after removing worst (should be >1)
			final double     smpl_fract, // Number of friends among all neighbors
			final int        smpl_num_narrow,   //         = 3;      // Number after removing worst (should be >1)
			final double     max_adiff,  // Maximal absolute difference betweenthe center tile and friends
			final double     max_rdiff, //  Maximal relative difference between the center tile and friends
			final double     max_atilt, //  = 2.0; // pix per tile
			final double     max_rtilt, //  = 0.2; // (pix / disparity) per tile
			final double     smpl_arms, //         = 0.1;    // Maximal RMS of the remaining tiles in a sample
			final double     smpl_rrms,        //      = 0.005;  // Maximal RMS/disparity in addition to smplRms
			final double     damp_tilt, //   =     0.001; // Tilt cost for damping insufficient plane data
			final double     rwsigma,           //  = 0.7; // influence of far neighbors diminish as a Gaussian with this sigma
			final double     rwsigma_narrow,    //  = used to determine initial tilt
			final double     center_weight,     // use center tile too (0.0 - do not use)
			final boolean    use_alt,           // use tiles from other scans if they fit better
			final double     goal_fraction_rms, // Try to make rms to be this fraction of maximal acceptable by removing outliers
			final double     boost_low_density, // 0 - strength is proportional to 1/density, 1.0 - same as remaining tiles
			final int        fourq_min,         // each of the 4 corners should have at least this number of tiles.
			final int        fourq_gap,         // symmetrical vertical and horizontal center areas that do not belong to any corner
			final int        dbg_x,
			final int        dbg_y,
			final int        debugLevel
    		){
		final int num_tiles = biCamDSI.tnImage.getSizeX()*biCamDSI.tnImage.getSizeY();
		double [][] ds0 = 	getDisparityStrength( // FIXME
	    		false,   // final boolean only_strong,
	    		false,   // final boolean only_trusted,
	    		true) ;  // final boolean only_enabled);


    	double [][] ds = new double[2][num_tiles];
    	for (int i = 0; i < num_tiles; i++) ds[0][i] = Double.NaN;
//    	double boost_low_density = 0.8; // 1.0; //0.2;
		  suggestNewScan(
				  area_of_interest,  // final boolean [] area_of_interest,
				  disparityStrength, // final double [][] disparityStrength,
				  trusted_strength,  // final double     trusted_strength, // trusted correlation strength
				  strength_rfloor,   // final double     strength_rfloor,   // strength floor - relative to trusted
				  true,              // final boolean    discard_cond,      // consider conditionally trusted tiles (not promoted to trusted) as empty
				  true,              // final boolean    discard_weak,      // consider conditionally trusted tiles (not promoted to trusted) as empty
				  true,              // final boolean    discard_strong,      // consider conditionally trusted tiles (not promoted to trusted) as empty
				  strength_pow,      // final double     strength_pow,      // raise strength-floor to this power
				  smpl_radius,       // final int        smpl_radius,
				  smpl_num,          // final int        smpl_num,   //         = 3;      // Number after removing worst (should be >1)
				  smpl_fract,        // final double     smpl_fract, // Number of friends among all neighbors
				  smpl_num_narrow,   // final int        smpl_num_narrow,   //         = 3;      // Number after removing worst (should be >1)
				  max_adiff,         // final double     max_adiff,  // Maximal absolute difference betweenthe center tile and friends
				  max_rdiff,         // final double     max_rdiff, //  Maximal relative difference between the center tile and friends
				  max_atilt,         // final double     max_atilt, //  = 2.0; // pix per tile
				  max_rtilt,         // final double     max_rtilt, //  = 0.2; // (pix / disparity) per tile
				  smpl_arms,         // final double     smpl_arms, //         = 0.1;    // Maximal RMS of the remaining tiles in a sample
				  smpl_rrms,         // final double     smpl_rrms,        //      = 0.005;  // Maximal RMS/disparity in addition to smplRms
				  damp_tilt,         // final double     damp_tilt, //   =     0.001; // Tilt cost for damping insufficient plane data
				  rwsigma,           // final double     rwsigma,           //  = 0.7; // influence of far neighbors diminish as a Gaussian with this sigma
				  rwsigma_narrow,    // final double     rwsigma_narrow,    //  = used to determine initial tilt
				  0.0,               // final double     new_diff,            // minimal difference between the new suggested and the already tried/measured one
				  false,             // final boolean    remove_all_tried,  // remove from suggested - not only disabled, but all tried
				  center_weight,     // final double     center_weight,     // use center tile too (0.0 - do not use)
				  use_alt,           // final boolean    use_alt,           // use tiles from other scans if they fit better
				  boost_low_density, // final double     boost_low_density, // 0 - strength is proportional to 1/density, 1.0 - same as remaining tiles
				  goal_fraction_rms, // final double     goal_fraction_rms, // Try to make rms to be this fraction of maximal acceptable by removing outliers
				  ds,                // final double [][]  smooth_ds,   // optionally fill strength array when used for smoothing DSI
				  fourq_min,         // final int        fourq_min,         // each of the 4 corners should have at least this number of tiles.
				  fourq_gap,         // final int        fourq_gap,         // symmetrical vertical and horizontal center areas that do not belong to any corner
				  dbg_x,             // final int        dbg_x,
				  dbg_y,             // final int        dbg_y,
				  debugLevel);       // final int        debugLevel);
		for (int nTile = 0; nTile < num_tiles; nTile++) {
			if ((ds0[1][nTile] > 0.0) && (
					(ds[0][nTile] <= 0.0) ||
					!discard_unreliable ||
					(strong_trusted[nTile] && !discard_strong) ||
					(trusted[nTile] && !discard_weak) ||
					(ds[0][nTile] <  min_disparity))) {
				ds[0][nTile] = ds0[0][nTile];
				ds[1][nTile] = ds0[1][nTile];
			}
		}
    	return ds;
    }
	public void showScan(String title) {
		showScan(title,null);
	}

	public void showScan(String title, double [][] ext_ds) {
		String [] titles= {
				"ext_disp",          //    0
				"all",               // 0  1
				"enabled",           // 1  2
				"cond_trusted",      // 2  3
				"weak trusted",      // 3  4
				"strong trusted",    // 4  5
				"measured",          // 5  6
				"suggested",         // 6  7
				"ext strength",      //    8
				"measured strength", // 7  9
				"strength" };        // 8 10
		double [][] ds_all = getDisparityStrength(
	    		false,   // final boolean only_strong,
	    		false,   // final boolean only_trusted,
	    		false) ; // final boolean only_enabled);
		double [][] ds = getDisparityStrength(
	    		false,   // final boolean only_strong,
	    		false,   // final boolean only_trusted,
	    		true) ; // final boolean only_enabled);

		double [][] dbg_data = new double[titles.length][];
		dbg_data[ 6] = this.disparity_measured;
		dbg_data[ 1] = ds_all[0];
		dbg_data[ 9] = this.strength_measured;
		dbg_data[10] = ds_all[1];
		dbg_data[ 2] = ds[0];
		if (ext_ds != null) {
			dbg_data[ 0] = ext_ds[0];
			dbg_data[ 8] = ext_ds[1];
		}

		if (this.cond_trusted != null) {
			dbg_data[3] = ds[0].clone();
			for (int i = 0; i < this.cond_trusted.length; i++) if (!cond_trusted[i]) dbg_data[3][i] = Double.NaN;
		}
		if (this.trusted != null) {
			dbg_data[4] = ds[0].clone();
			for (int i = 0; i < this.trusted.length; i++) if (!trusted[i]) dbg_data[4][i] = Double.NaN;
		}
		if (this.strong_trusted != null) {
			dbg_data[5] =  ds[0].clone();
			for (int i = 0; i < this.strong_trusted.length; i++) if (!strong_trusted[i]) dbg_data[5][i] = Double.NaN;
		}
		if (this.target_disparity != null) {
			dbg_data[7] = this.target_disparity.clone();
		}
		ShowDoubleFloatArrays.showArrays(
				dbg_data,
				biCamDSI.tnImage.getSizeX(),
				biCamDSI.tnImage.getSizeY(),
				true,
				title,
				titles);
	}

	/**
	 * Reduce outliers on DSI when multiple "refined" disparity values exist for the same tile and the strongest does not seem to be the best
	 * Each disparity solution is compared to the weighted average of the neighbors and the strength is divided by the difference from that
	 * average value, so the closest to the neighbors gets strength boost.
	 * @param str_floor absolute strength to subtract from the measured
	 * @param pf_disp_afloor offset disparity to add to the disparity difference to avoid division by 0 or small numbers
	 * @param pf_disp_rfloor realtive to the disparity portion of the offset
	 * @return number of replaced tiles
	 */
	public int copyFittestEnabled(
			final double  str_floor,      // absolute strength floor
			final double  pf_disp_afloor, // =            0.1;    // When selecting the best fit from the alternative disparities, divide by difference increased by this
			final double  pf_disp_rfloor) //  =            0.02;   // Increase pf_disp_afloor for large disparities
		{
		final TileNeibs  tnImage = biCamDSI.tnImage;
		final int num_tiles = biCamDSI.tnImage.getSizeX()*biCamDSI.tnImage.getSizeY();

		final double [][] ds = getDisparityStrength( // FIXME
	    		false,   // final boolean only_strong,
	    		false,   // final boolean only_trusted,
	    		true) ; // final boolean only_enabled);

		final Thread[] threads = ImageDtt.newThreadArray(biCamDSI.threadsMax);
		final AtomicInteger ai = new AtomicInteger(0);
		final int [] new_src = new int[num_tiles];
		final AtomicInteger num_changes = new AtomicInteger(0);
		int dbg_x = 157;
		int dbg_y = 212;
		int debugLevel = -10;
		final int dbg_tile = (debugLevel>-2)?(dbg_x + tnImage.sizeX*dbg_y):-1;

		ai.set(0);
		// find definitely trusted and conditionally trusted tiles
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				@Override
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) {
						if (nTile == dbg_tile) {
							System.out.println("copyFittestEnabled(): nTile="+nTile);
						}
						if (ds[1][nTile] > 0.0 ) { // && (src_index[nTile] != list_index)){ // should be already found
							int num_neib = 0;
							double sw = 0.0, swd = 0.0;
							double wdiag = 0.7;
							for (int dir = 0; dir <8; dir++) {
								int nTile1 = tnImage.getNeibIndex(nTile, dir);
								if ((nTile1 >= 0) && (ds[1][nTile1] > str_floor)) {
									double w = ds[1][nTile1] - str_floor;
									if ((dir & 1) != 0) w *= wdiag;
									sw += w;
									swd += ds[0][nTile1] * w;
									num_neib++;
								}
							}
							if (num_neib > 0) {
								double disp_mean = swd/sw;
								double disp_floor = pf_disp_afloor + disp_mean * pf_disp_rfloor;
								double disp_floor2 = disp_floor*disp_floor;

								int best_indx=-1;
								double best_strength =0.0;
								for (int indx = list_index; indx >= 0; indx--) { // include the latest measurement
									BiScan scan = biCamDSI.getBiScan(indx);
									if (scan.disabled_measurement[nTile]) { //  || (scan.src_index[nTile] != indx)){ // skip all but enabled
										continue;
									}
									// calculate effective strength
									double strength = scan.strength_measured[nTile] - str_floor;
									if (strength <= 0) {
										continue;
									}

									double diff = scan.disparity_measured[nTile] - disp_mean;
									double eff_strength = strength/Math.sqrt(diff*diff + disp_floor2);

									if (eff_strength > best_strength) {
										best_strength = eff_strength;
										best_indx = indx;
									}
								}
								if ((best_indx >= 0) &&  (best_indx != src_index[nTile])) { // not the same as already set
									new_src[nTile] = best_indx+1; // +1 so initial 0 will be "not set"
									num_changes.getAndIncrement();
								}
							}
						}
					}
				}
			};
		}
		ImageDtt.startAndJoin(threads);
		if (num_changes.get() > 0) {
			ai.set(0);
			// find definitely trusted and conditionally trusted tiles
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					@Override
					public void run() {
						for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) {
							if (nTile == dbg_tile) {
								System.out.println("copyFittestEnabled() 2 : nTile="+nTile);
							}
							if (new_src[nTile] > 0){
								int best_indx = new_src[nTile]-1;
								src_index[nTile] = best_indx;
							}
						}
					}
				};
			}
			ImageDtt.startAndJoin(threads);
		}
		// will need trusted* recalculated
		return num_changes.get();
	}

	/**
	 * Prefer thin FG (like thin poles) over textured BG (at first not directional), just prefer "good enough" FG over even stronger BG
	 * @param str_good_enough minimal strength for the FG tiles and neighbors
	 * @param min_FGtoBG minimal FG to BG disparity difference
	 * @param disp_atolerance absolute disparity difference for qualifying neighbors (in master camera pixels)
	 * @param disp_rtolerance add to tolerance for each pixel of disparity
	 * @param min_neib minimal number of neighbors that promoted tiles should have
	 * @return number of promoted FG tiles
	 */
	public int copyStrongFGEnabled(
			final double  str_good_enough, // absolute strength floor for good enough
			final double  min_FGtoBG,      // minimal disparity difference over
			final double  disp_atolerance, // =  0.1;    // Maximal absolute disparity difference to qualifying neighbor
			final double  disp_rtolerance, // =  0.02;   // Maximal relative (to absolute disparity) disparity difference to qualifying neighbor
			final int     min_neib)        // minimal number of qualifying neighbors to promote FG tile
		{
		final TileNeibs  tnImage = biCamDSI.tnImage;
		final int num_tiles = biCamDSI.tnImage.getSizeX()*biCamDSI.tnImage.getSizeY();

		final double [][] ds = getDisparityStrength( // used to comapre to see if re-arrangement is needed
	    		false,   // final boolean only_strong,
	    		false,   // final boolean only_trusted,
	    		true) ; // final boolean only_enabled);

		final Thread[] threads = ImageDtt.newThreadArray(biCamDSI.threadsMax);
		final AtomicInteger ai = new AtomicInteger(0);
		final int [] new_src = new int[num_tiles];
		final AtomicInteger num_changes = new AtomicInteger(0); // number of tiles modified to FG
		int dbg_x = 193;
		int dbg_y = 162;
		int debugLevel = -1;
		final int dbg_tile = (debugLevel>-1)?(dbg_x + tnImage.sizeX*dbg_y):-1;

		ai.set(0);
		// find definitely trusted and conditionally trusted tiles
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				@Override
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) {
						if (nTile == dbg_tile) {
							System.out.println("copyStrongFGEnabled(): nTile="+nTile);
						}
						if (ds[1][nTile] < 0) { // no good d/s for this tile
							continue;
						}
						// get list in descending disparity order
						ArrayList<DSIndex> ds_list = getTileDSList(
								nTile, // int           nTile,
								false, // final boolean only_strong,
								false, // final boolean only_trusted,
								true); // final boolean only_enabled)
						if (ds_list.size() < 1) {
							continue;
						}

						// find strongest tile closer than default disparity
						for (int indx = 0; indx < ds_list.size(); indx++) {
							DSIndex dsi = ds_list.get(indx);
							if (dsi.disparity < (ds[0][nTile] + min_FGtoBG)){
								break; // not sufficiently closer than default;
							}
							if (dsi.strength > str_good_enough){
								double  disp_tolerance = disp_atolerance + dsi.disparity * disp_rtolerance; // disparity tolerance
								// see if it has strong enough neighbors with close disparity
								int num_neib = 0;
								for (int dir = 0; dir < 8; dir++) {
									int nTile1 = tnImage.getNeibIndex(nTile, dir);
									if (nTile1 > 0) {
										ArrayList<DSIndex> ds_neib = getTileDSList(
												nTile1, // int           nTile,
												false, // final boolean only_strong,
												false, // final boolean only_trusted,
												true); // final boolean only_enabled)
										for (DSIndex dsi_neib: ds_neib) {
											if ((dsi_neib.strength > str_good_enough) &&
													(Math.abs(dsi_neib.disparity - dsi.disparity ) <= disp_tolerance)) {
												num_neib++;
												break;
											}
										}
									}
								}
								if (num_neib > 0) {
									new_src[nTile] = dsi.scan_index + 1;
									num_changes.getAndIncrement();
									break; // for (int indx = 0; indx < ds_list.size(); indx++)
								}
							}
						}
					}
				}
			};
		}
		ImageDtt.startAndJoin(threads);
		if (num_changes.get() > 0) {
			ai.set(0);
			// find definitely trusted and conditionally trusted tiles
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					@Override
					public void run() {
						for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) {
							if (nTile == dbg_tile) {
								System.out.println("copyStrongFGEnabled() 2 : nTile="+nTile);
							}
							if (new_src[nTile] > 0){
								int best_indx = new_src[nTile]-1;
								src_index[nTile] = best_indx;
							}
						}
					}
				};
			}
			ImageDtt.startAndJoin(threads);
		}
		// will need trusted* recalculated
		return num_changes.get();
	}


	/**
	 * Copy data (if the current was not measured) from one of the previous scans - strongest that is not disabled. If last_priority is true
	 * the latest not disabled scan will be used, even if it is not the strongest
	 * @param last_priority
	 */

	public void copyLastStrongestEnabled(
			final boolean last_priority) // use last if exists and is not disabled
		{
		final int num_tiles = biCamDSI.tnImage.getSizeX()*biCamDSI.tnImage.getSizeY();
		final Thread[] threads = ImageDtt.newThreadArray(biCamDSI.threadsMax);
		final AtomicInteger ai = new AtomicInteger(0);
//		final boolean [] cond_trusted = new boolean [num_tiles];
		strong_trusted = new boolean [num_tiles];
		trusted =        new boolean [num_tiles];
		cond_trusted =   new boolean [num_tiles];

		ai.set(0);
		// find definitely trusted and conditionally trusted tiles
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				@Override
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) if (strength_measured[nTile] <= 0.0 ){ // keep last measurement for a while, even if it is not the best
						int best_indx=-1;
						boolean latest = true;
						double best_strength =0.0;
						for (int indx = list_index-1; indx >= 0; indx--) { // no need to try the latest - it is empty (strength_measured[nTile] <= 0.0 )
							BiScan scan = biCamDSI.getBiScan(indx);
							if (scan.disabled_measurement[nTile] || (scan.src_index[nTile] != indx)){ // skip all but enabled sources
								continue;
							}
							if (scan.strength_measured[nTile] > best_strength) {
								best_strength = scan.strength_measured[nTile];
								best_indx = indx;
							}
							if (last_priority && latest) {
								break; // best_indx should be set correctly, as strength > 0.0
							}
							latest = false; // first not disabled with strength>0 gets here
						}
						if (best_indx >= 0) {
							src_index[nTile] = best_indx;
						}
					}
				}
			};
		}
		ImageDtt.startAndJoin(threads);
	}




	public void showScans(String title) {
	}
	/**
	 * Suggest disparities to try for the tiles in poorly textured areas by fitting planes in DSI
	 * calcTrusted should be called before to set up trusted/cond_trusted tiles
	 * suggested tiles will be compared against and made sure they differ by more than a specified margin
	 * 1) current measured (refined) disparity value
	 * 2) target disparity that lead to the current measurement after refinement
	 * 3) any other disable measurement
	 * 4) any target disparity that lead to the disabled measurement
	 * @param area_of_interest - limit results to these tiles (if provided)
	 * @param disparityStrength - a pair of array or null. If null, will calculate fro the current scan
	 *        if not null - use as is
	 * @param trusted_strength strength to trust unconditionally
	 * @param strength_rfloor strength floor to subtract as a fraction of the trusted strength
	 * @param discard_cond if true may suggest new disparities for conditionally trusted tiles
	 * @param discard_weak if true may suggest new disparities over trusted weak tiles
	 * @param discard_stron if true may suggest new disparities over any tile
	 * @param strength_pow raise strength to this power (normally just 1.0)
	 * @param smpl_radius sample "radius", square side is  2 * smpl_radius + 1
	 * @param smpl_num minimal absolute number of samples required to try fit a plane and validate a tile
	 * If smpl_num == 0, faster calculation (single pass) using only *_narrow settings
	 * @param smpl_fract minimal fraction number of the neighbor samples that fit the rms filter required to try fit a plane and validate a tile
	 * @param smpl_num_narrow minimal absolute number of samples for preliminary fitting plane to trhe center area
	 * @param max_adiff maximal absolute difference from the center tile for initial neighbors selection
	 * @param max_rdiff maximal (additional) relative (to tile disparity) difference from the center tile for initial neighbors selection
	 * @param max_atilt maximal absolute tilt (pix/tile) for the tilted planes to fit
	 * @param max_rtilt maximal relative tilt (pix/tile per disparity pixel). min(max_rtilt*disparity, max_atilt) will be used
	 * @param smpl_arms maximal absolute rms of the weighted remaining samples for the successful plane fitting
	 * @param smpl_rrms maximal relative (additional)rms of the weighted remaining samples for the successful plane fitting
	 * @param damp_tilt regularization value to handle planes if the remaining samples are co-linear (or just a single tile)
	 * @param rwsigma weight Gaussian sigma to reduce influence of far tiles relative to smpl_radius
	 * @param rwsigma_narrow Gaussian sigma for the preliminary plain fitting using the closest tiles ~= 1/smpl_radius
	 * @param new_diff minimal difference between the new suggested and the already tried/measured one
	 * @param remove_all_tried remove from suggested - not only disabled, but all tried
	 * @param center_weight weight of the tile itself (0.0 - do not use). Should be set to 0.0 for suggesting, >0 - for "smoothing"
	 * @param use_alt use tiles from other scans if they fit better
	 * @param goal_fraction_rms try to make rms to be this fraction of maximal acceptable by removing outliers
	 * @param boost_low_density 0.0 - strength is proportional to 1/density, 1.0 - same as remaining tiles
	 * @param smooth_ds optionally fill disparity/strength  instead of the target_disparity
	 * @param fourq_min each of the 4 corners should have at least this number of tiles.
	 * @param fourq_gap symmetrical vertical and horizontal center areas that do not belong to any corner
	 * @param dbg_x tileX to debug
	 * @param dbg_y tileY to debug
	 * @param debugLevel debug level
	 * @return number of new tiles to measure in the  array of suggested disparities - Double.NaN - nothing suggested
	 *  for the tile. May need additional filtering to avoid suggested already tried disparities
	 */

	int  suggestNewScan(
			final boolean [] area_of_interest,
			final double [][] disparityStrength,
		    final double     trusted_strength, // trusted correlation strength
			final double     strength_rfloor,   // strength floor - relative to trusted
		    final boolean    discard_cond,      // consider conditionally trusted tiles (not promoted to trusted) as empty
		    final boolean    discard_weak,      // consider weak trusted tiles (not promoted to trusted) as empty
		    final boolean    discard_strong,    // suggest new disparities even for strong tiles
			final double     strength_pow,      // raise strength-floor to this power
			final int        smpl_radius,
			final int        smpl_num,   //         = 3;      // Number after removing worst (should be >1)
			final double     smpl_fract, // Number of friends among all neighbors
			final int        smpl_num_narrow,   //         = 3;      // Number after removing worst (should be >1)
			final double     max_adiff,  // Maximal absolute difference between the center tile and friends
			final double     max_rdiff, //  Maximal relative difference between the center tile and friends
			final double     max_atilt, //  = 2.0; // pix per tile
			final double     max_rtilt, //  = 0.2; // (pix / disparity) per tile
			final double     smpl_arms, //         = 0.1;    // Maximal RMS of the remaining tiles in a sample
			final double     smpl_rrms,        //      = 0.005;  // Maximal RMS/disparity in addition to smplRms
			final double     damp_tilt, //   =     0.001; // Tilt cost for damping insufficient plane data
			final double     rwsigma,           //  = 0.7; // influence of far neighbors diminish as a Gaussian with this sigma
			final double     rwsigma_narrow,    //  = used to determine initial tilt
			final double     new_diff,          // minimal difference between the new suggested and the already tried/measured one
			final boolean    remove_all_tried,  // remove from suggested - not only disabled, but all tried
			final double     center_weight,     // use center tile too (0.0 - do not use)
			final boolean    use_alt,           // use tiles from other scans if they fit better
			final double     goal_fraction_rms, // Try to make rms to be this fraction of maximal acceptable by removing outliers
			final double     boost_low_density, // 0 - strength is proportional to 1/density, 1.0 - same as remaining tiles
			final double [][] smooth_ds,        // optionally fill disparity/strength  instead of the target_disparity
			final int        fourq_min,         // each of the 4 corners should have at least this number of tiles.
			final int        fourq_gap,         // symmetrical vertical and horizontal center areas that do not belong to any corner
			final int        dbg_x,
			final int        dbg_y,
			final int        debugLevel
			) {
		final TileNeibs  tnImage = biCamDSI.tnImage;
		final int dbg_tile = (debugLevel > 0)?(dbg_y * tnImage.sizeX + dbg_x): -1;
		final double     wsigma = rwsigma*smpl_radius;
		final double     wsigma_narrow = rwsigma_narrow*smpl_radius;
		final int        smpl_side = 2 * smpl_radius + 1; // Sample size (side of a square)
		final int        smpl_len = smpl_side*smpl_side;
		final int        smpl_center = (smpl_side + 1) * smpl_radius;
		// prepare window
		final double [][] weights =        new double [smpl_radius + 1][smpl_radius + 1];
		final double [][] weights_narrow = new double [smpl_radius + 1][smpl_radius + 1];
		{ // normalize
			for (int i = 0; i <= smpl_radius; i++) {
				for (int j = i; j <= smpl_radius; j++) {
					weights[i][j] = (wsigma >0.0) ?Math.exp(-(i*i+j*j)/(2*wsigma*wsigma)):1.0;
					weights[j][i] = weights[i][j];
					weights_narrow[i][j] = (wsigma_narrow >0.0) ?Math.exp(-(i*i+j*j)/(2*wsigma_narrow*wsigma_narrow)):1.0;
					weights_narrow[j][i] = weights_narrow[i][j];
				}
			}
			weights[0][0] *=        center_weight;
			weights_narrow[0][0] *= center_weight;
			double sw_full = 0.0, sw_narrow = 0.0;
			for (int i = 0; i <= smpl_radius; i++) {
				for (int j = i; j <= smpl_radius; j++) {
					sw_full +=   weights[i][j];
					sw_narrow += weights_narrow[i][j];
				}
			}
			double k_full = 1.0/sw_full;
			double k_narrow = 1.0/sw_narrow;
			for (int i = 0; i <= smpl_radius; i++) {
				for (int j = i; j <= smpl_radius; j++) {
					weights[i][j] *=k_full;
					weights_narrow[i][j]*=k_narrow;
				}
			}
		}
		final int [] fourq_corner = new int[smpl_len];
		for (int i = 0; i < smpl_side; i++) {
			for (int j = 0; j < smpl_side; j++) {
				int indx = i* smpl_side + j;
				if     (((i > (smpl_radius - fourq_gap)) && (i < (smpl_radius + fourq_gap))) ||
						((j > (smpl_radius - fourq_gap)) && (j < (smpl_radius + fourq_gap)))){
					fourq_corner[indx] = 4; // will not be used

				} else {
					fourq_corner[indx] = ((i > smpl_radius) ? 2 : 0) + ((j > smpl_radius) ? 1 : 0); //122
				}
			}
		}


		final double [][] ds = (disparityStrength != null) ? disparityStrength: getDisparityStrength(
	    		false,   // final boolean only_strong,
	    		false,   // final boolean only_trusted,
	    		true) ; // final boolean only_enabled);
		final int num_tiles = tnImage.getSizeX()*tnImage.getSizeY();
		final double     strength_floor = trusted_strength * strength_rfloor;

		final boolean [] trusted_sw = discard_weak ? (this.strong_trusted) : (discard_cond ? this.trusted: this.cond_trusted);

		final Thread[] threads = ImageDtt.newThreadArray(biCamDSI.threadsMax);
		final AtomicInteger ai = new AtomicInteger(0);
//		final double [] suggested_disparity = new double [num_tiles];
		target_disparity = new double [num_tiles];
		for (int i = 0; i < num_tiles; i++) target_disparity[i] = Double.NaN;
//		cond_trusted and trusted should be set;
		ai.set(0);
//		final BiScan this_scan = this;
		final AtomicInteger num_new = new AtomicInteger(0);
		// find definitely trusted and conditionally trusted tiles
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				@Override
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) if (
							((area_of_interest == null) || area_of_interest[nTile]) && (discard_strong || !trusted_sw[nTile])){
						boolean debug = nTile == dbg_tile;
						if (debug) {
							System.out.println("suggestNewScan(): debbugging nTile="+nTile);
							System.out.println("suggestNewScan(): debbugging nTile="+nTile);
						}
						// Select all neighbors, then filter
						double [] smpl_d =           new double  [smpl_len];
						double [] smpl_w =           new double  [smpl_len];
						double [] smpl_w_narrow =    new double  [smpl_len];
						double [] smpl_p =           new double  [smpl_len]; // plane disparity,
						double [] smpl_wsw= smpl_w_narrow;
						double [][] weights_sw = weights_narrow;

						int nall = 0;
						double sw = 0, swd = 0;
						for (int dy = -smpl_radius; dy <= smpl_radius; dy++) {
							int ady = (dy > 0)? dy:(-dy);
							for (int dx = -smpl_radius; dx <= smpl_radius; dx++) { // if ((dx != 0) || (dy != 0)){
								int nTile1 =  tnImage.getNeibIndex(nTile, dx, dy);
								if ((nTile1 >= 0) && trusted[nTile1]) { // weak trusted OK
									nall++;
									int adx = (dx > 0)? dx:(-dx);
									int smpl_indx = smpl_center + dy*smpl_side + dx;
									double w = ds[1][nTile1]-strength_floor;
									if ( w > 0) {
										if (strength_pow != 1.0) {
											w = Math.pow(w, strength_pow);
										}

										smpl_d[smpl_indx] =       ds[0][nTile1];
										smpl_w[smpl_indx] =        w * weights[ady][adx];
										smpl_w_narrow[smpl_indx] = w * weights_narrow[ady][adx];
										sw += smpl_w_narrow[smpl_indx];
										swd += smpl_w_narrow[smpl_indx]* smpl_d[smpl_indx];
									}
								}
							}
						}
						if (debug) {
							System.out.println("suggestNewScan(): sw="+sw);
						}

						if (sw == 0.0) {
							continue; //
						}
						double disp_mean = swd/sw; // preliminary reference disparity
						double max_tilt = max_rtilt * disp_mean;
						if (max_tilt > max_atilt) {
							max_tilt = max_atilt;
						}

						int fin_samples= (int) ( nall * smpl_fract);
						// smpl_num == 0 - special (fast) case do not use wide selection at all
						if ((smpl_num == 0) || (fin_samples < smpl_num)) fin_samples = smpl_num;

						// fit plane to mostly centertiles
						int nsmpls = nall;
						if (nsmpls < smpl_num_narrow) { // no tiles even to start
							continue; //
						}
						double max_rms = smpl_arms + smpl_rrms * disp_mean; // do not need to wait fro the final disparity for this estimate
						double [] fit_rslt = fitPlaneRemoveOutliers(
								smpl_radius,        // int                     smpl_radius,
								max_tilt,           // double                  max_tilt,
								damp_tilt,          // double                  damp_tilt, //   =     0.001; // Tilt cost for damping insufficient plane data
								true,               // boolean                 full_plane,
								smpl_d,             // double []               smpl_d,
								smpl_w_narrow,      // double []               smpl_w, // will be modified,
								smpl_p,             // double []               smpl_p, // will be set if provided
								smpl_num_narrow,    // int                     fin_samples, // remove until this number remain
								goal_fraction_rms*max_rms, // double                  fin_rms,
								false,              // boolean                 keep_center, // do not remove center tile - it is the tile that should be verified by neighbors
								fourq_min,          // int                     fourq_min,         // each of the 4 corners should have at least this number of tiles.
								fourq_corner,       // int [] 	               fourq_corner, //  array specifying corner number (0..3), -1 - gap. null when not used
								debugLevel);        // int                     debugLevel)
//						if ( (fit_rslt == null) ||  (fit_rslt[0] > (smpl_arms + smpl_rrms * fit_rslt[1]))){
						if ( (fit_rslt == null) ||  (fit_rslt[0] > max_rms)){
							continue; // narrow selection - too high rms
						}
						max_rms = smpl_arms + smpl_rrms * fit_rslt[1]; // updated center disparity and so rms
						disp_mean = fit_rslt[1]; // smpl_p[smpl_center]; // center of the fitted plane
						// try to use alternatives for the discarded (not only) tiles
						if (use_alt) {
							int nnew = findBetterFitToPlane( // ; // new assignments
									smpl_radius,     // int       smpl_radius,
									nTile,           // int       nTile,
									strength_floor,  // double    strength_floor,
									strength_pow,    // double    strength_pow,
									weights,         // double [][] weights,
									weights_narrow,  // double [][] weights_narrow,
									smpl_w,          // double [] smpl_w,
									smpl_w_narrow,   // double [] smpl_w_narrow,
									smpl_d,          // double [] smpl_d,
									smpl_p);         // double [] smpl_p);

							if (nnew > 0) { // there were some changes, recalculate narrow fit plane
								fit_rslt = fitPlaneRemoveOutliers(
										smpl_radius,        // int                     smpl_radius,
										max_tilt,           // double                  max_tilt,
										damp_tilt,          // double                  damp_tilt, //   =     0.001; // Tilt cost for damping insufficient plane data
										true,               // boolean                 full_plane,
										smpl_d,             // double []               smpl_d,
										smpl_w_narrow,      // double []               smpl_w, // will be modified,
										smpl_p,             // double []               smpl_p, // will be set if provided
										smpl_num_narrow,    // int                     fin_samples, // remove until this number remain
										goal_fraction_rms*max_rms,            // double                  fin_rms,
										false,              // boolean                 keep_center, // do not remove center tile - it is the tile that should be verified by neighbors
										fourq_min,          // int                     fourq_min,         // each of the 4 corners should have at least this number of tiles.
										fourq_corner,       // int [] 	               fourq_corner, //  array specifying corner number (0..3), -1 - gap. null when not used
										debugLevel);        // int                     debugLevel)
//								if ( (fit_rslt == null) ||  (fit_rslt[0] > (smpl_arms + smpl_rrms * fit_rslt[1]))){
								if ( (fit_rslt == null) ||  (fit_rslt[0] > max_rms)){
									continue; // narrow selection - too high rms
								}
								disp_mean = fit_rslt[1]; // smpl_p[smpl_center]; // center of the fitted plane
							}
// plane may have changed, look for the best fit in history again
							 nnew = findBetterFitToPlane( // ; // new assignments
										smpl_radius,     // int       smpl_radius,
										nTile,           // int       nTile,
										strength_floor,  // double    strength_floor,
										strength_pow,    // double    strength_pow,
										weights,         // double [][] weights,
										weights_narrow,  // double [][] weights_narrow,
										smpl_w,          // double [] smpl_w,
										smpl_w_narrow,   // double [] smpl_w_narrow,
										smpl_d,          // double [] smpl_d,
										smpl_p);         // double [] smpl_p);
						} //if (use_alt) {

						// re-select tiles to fit the plane and use wide weights
						double max_diff = max_adiff + max_rdiff * disp_mean; // no provisions for tilt (or add a fraction)?
						nsmpls = 0;
						for (int indxs = 0; indxs < smpl_len; indxs++) if (smpl_w[indxs]>0){
							if (Math.abs(smpl_d[indxs] - smpl_p[indxs]) < max_diff) {
								nsmpls++;
							} else {
								smpl_w[indxs] = 0.0;
							}
						}
						if (fin_samples > 0) {

							if (nsmpls < fin_samples) { // no tiles even to satrt
								continue; //
							}
							fit_rslt = fitPlaneRemoveOutliers(
									smpl_radius,        // int                     smpl_radius,
									max_tilt,           // double                  max_tilt,
									damp_tilt,          // double                  damp_tilt, //   =     0.001; // Tilt cost for damping insufficient plane data
									true,               // boolean                 full_plane,
									smpl_d,             // double []               smpl_d,
									smpl_w,             // double []               smpl_w, // will be modified,
									smpl_p,             // double []               smpl_p, // will be set if provided
									fin_samples,        // int                     fin_samples, // remove until this number remain
									goal_fraction_rms*max_rms,            // double                  fin_rms,
									false,              // boolean                 keep_center, // do not remove center tile - it is the tile that should be verified by neighbors
									fourq_min,          // int                     fourq_min,         // each of the 4 corners should have at least this number of tiles.
									fourq_corner,       // int [] 	               fourq_corner, //  array specifying corner number (0..3), -1 - gap. null when not used
									debugLevel);        // int                     debugLevel)
							smpl_wsw = smpl_w;
							weights_sw = weights;
							//						if ( (fit_rslt == null) ||  (fit_rslt[0] > (smpl_arms + smpl_rrms * fit_rslt[1]))){
							if ( (fit_rslt == null) ||  (fit_rslt[0] > max_rms)){
								continue; // narrow selection - too high rms
							}
						}
						boolean valid_suggestion = true;
						if ((disparityStrength == null) && (new_diff > 0.0)) {
							if (Math.abs(fit_rslt[1] - ds[0][nTile]) < new_diff) { // suggested is too close to already measured
								continue; // already measured for this tile
							}
							// compare to the previous suggestion
							int previous_indx = ((src_index[nTile] < 0)? list_index:src_index[nTile]) -1;
							double previous_target = (previous_indx >= 0)? biCamDSI.getBiScan(previous_indx).target_disparity[nTile]:Double.NaN; // Nothing is known about the target of the 0-scan
							if (Math.abs(fit_rslt[1] - previous_target) < new_diff) { // suggested is too close to already suggested and result - disabled
								continue; // already measured for this tile
							}
							// see if close one was already disabled
							// do not compare with the scans that were not disabled - re-try them?remove_all_tried
							// If disparityStrength[][] is provided, do not use history. If difference is <= 0 - also no sense to look there
							//							for (BiScan other_scan:biCamDSI.biScans) if (other_scan.disabled_measurement[nTile] || remove_all_tried) {
							for (BiScan other_scan:biCamDSI.biScans) if ( (other_scan.list_index <= list_index) && (other_scan.disabled_measurement[nTile] || remove_all_tried)) {

								//							int other_indx = (other_scan.src_index[nTile] < 0)? other_scan.list_index:other_scan.src_index[nTile];
								//							double other_disparity = biCamDSI.getBiScan(other_indx).disparity_measured[nTile];
								double other_disparity = other_scan.disparity_measured[nTile];
								if (Math.abs(fit_rslt[1] - other_disparity) < new_diff) { // suggested is too close to already measured and disabled
									valid_suggestion = false;
									break; // already measured for this tile
								}
								int other_indx = other_scan.list_index;
								double other_target = (other_indx > 0)? biCamDSI.getBiScan(other_indx - 1).target_disparity[nTile]:Double.NaN; // Nothing is known about the target of the 0-scan
								if (Math.abs(fit_rslt[1] - other_target) < new_diff) { // suggested is too close to already suggested and result - disabled
									valid_suggestion = false;
									break; // already measured for this tile
								}
							}
						}
						if (valid_suggestion) {
							num_new.getAndIncrement();
							if (smooth_ds == null) { // fill target disparity
								target_disparity[nTile] = fit_rslt[1];
							} else { // this method is used to provide a filtered DSI - no changes to the target_disparity
								smooth_ds[0][nTile] = fit_rslt[1];
								double s0 = 0, s1 = 0;
								for (int dy = -smpl_radius; dy <= smpl_radius; dy++) {
									int ady = (dy > 0)? dy:(-dy);
									for (int dx = -smpl_radius; dx <= smpl_radius; dx++) { // if ((dx != 0) || (dy != 0)){
										int adx = (dx > 0)? dx:(-dx);
										int smpl_indx = smpl_center + dy*smpl_side + dx;
										if (smpl_wsw[smpl_indx] > 0.0) { // either smpl_w_narrow or smpl_w
											s0+= weights_sw[ady][adx];
//											s1+= weights[ady][adx] * smpl_w[smpl_indx];
											s1+= smpl_wsw[smpl_indx]; // already was multiplied by weights[ady][adx]
										}
									}
								}
								//boost_low_density, // false weight of low density tiles is reduced, true - boosted
								double w = s1/s0;
//								if (boost_low_density > 0.0) {
//									w/= Math.pow(s0, boost_low_density);
//								}
								if (strength_pow != 1.0) {
									w = Math.pow(w, 1.0 / strength_pow);
								}
								w += strength_floor;
								smooth_ds[1][nTile] = w;
								if (Double.isNaN(w)) {
									System.out.println("suggestNewScan(): nTile="+nTile+" w="+w);
									System.out.println("suggestNewScan(): nTile="+nTile+" w="+w);
								}
//								if (nTile == 66945) {
//									System.out.println("suggestNewScan(): nTile="+nTile+" w="+w);
//									System.out.println("suggestNewScan(): nTile="+nTile+" w="+w);
//								}

							}
						}
					}
				}
			};
		}
		ImageDtt.startAndJoin(threads);
		// remove duplicates from what was suggested or measured before
		return num_new.get();
	}



    private int findBetterFitToPlane(
    		int       smpl_radius,
    		int       nTile,
    		double    strength_floor,
    		double    strength_pow,
    		double [][] weights,
    		double [][] weights_narrow,
    		double [] smpl_w,
    		double [] smpl_w_narrow,
    		double [] smpl_d,
    		double [] smpl_p
    		)
    {
		final TileNeibs  tnImage = biCamDSI.tnImage;
		final int        smpl_side = 2 * smpl_radius + 1; // Sample size (side of a square)
//		final int        smpl_len = smpl_side*smpl_side;
		final int        smpl_center = (smpl_side + 1) * smpl_radius;
		int nnew = 0; // new assignments
		for (int dy = -smpl_radius; dy <= smpl_radius; dy++) {
			int ady = (dy > 0)? dy:(-dy);
			for (int dx = -smpl_radius; dx <= smpl_radius; dx++){
				int adx = (dx > 0)? dx:(-dx);
				int smpl_indx = smpl_center + dy*smpl_side + dx;
				if  (smpl_w [smpl_indx] > 0.0) { // is in "untouched" weights
					int nTile1 =  tnImage.getNeibIndex(nTile, dx, dy);
//					if ((nTile1 >= 0) && trusted[nTile1]) { // weak trusted OK
					if (nTile1 >= 0) { // will use any strength > 0
						// see if there is a measurement that fits better to the plane
						int best_indx=-1;
						double best_diff = Math.abs(smpl_d[smpl_indx] - smpl_p[smpl_indx]);
						for (int scan_indx = list_index; scan_indx >= 0; scan_indx--) { // include the latest measurement
							BiScan scan = biCamDSI.getBiScan(scan_indx);
							if (scan.disabled_measurement[nTile]) { //  || (scan.src_index[nTile] != indx)){ // skip all but enabled
								continue;
							}
							double disp = scan.disparity_measured[nTile1];
							if (Double.isNaN(disp)) {
								continue;
							}
							if (scan.strength_measured[nTile1] <= strength_floor) {
								continue;
							}
							double diff = Math.abs(disp - smpl_p[smpl_indx]);
							if ((best_indx < 0) || (diff < best_diff)){
								best_indx = scan_indx;
								best_diff = diff;
							}
						}
						if (best_indx >= 0) {
							BiScan scan = biCamDSI.getBiScan(best_indx);
							smpl_d[smpl_indx] = scan.disparity_measured[nTile1];
							double w =          scan.strength_measured[nTile1] -strength_floor ;
							if (w > 0.0) { // should be anyway
								if (strength_pow != 1.0) {
									w = Math.pow(w, strength_pow);
								}
								smpl_w[smpl_indx] =        w * weights[ady][adx];
								smpl_w_narrow[smpl_indx] = w * weights_narrow[ady][adx];
								nnew++;
							}
						}
					}
				}
			}
		}
		return nnew;
    }


// Simple version for non-flat strong areas - try duplicating neighbor
	int  suggestNewScan(
			final int []     dxy,               //up,down,right,left
		    final boolean    discard_cond,      // consider conditionally trusted tiles (not promoted to trusted) as empty
		    final boolean    discard_weak,      // consider weak trusted tiles (not promoted to trusted) as empty
		    final boolean    discard_strong,    // suggest new disparitieas even for strong tiles
			final double     new_diff,          // minimal difference between the new suggested and the already tried/measured one
			final boolean    remove_all_tried,  // remove from suggested - not only disabled, but all tried
			final int        dbg_x,
			final int        dbg_y,
			final int        debugLevel
			) {
//		int dbg_x = 193;
//		int dbg_y = 162;
//		int debugLevel = -1;
//		final int dbg_tile = (debugLevel>-2)?(dbg_x + tnImage.sizeX*dbg_y):-1;

		final TileNeibs  tnImage = biCamDSI.tnImage;
		final double [][] ds = getDisparityStrength( // FIXME
	    		false,   // final boolean only_strong,
	    		false,   // final boolean only_trusted,
	    		true) ;  // final boolean only_enabled);

		final int num_tiles = tnImage.getSizeX()*tnImage.getSizeY();

		final boolean [] trusted_sw = discard_weak ? (this.strong_trusted) : (discard_cond ? this.trusted: this.cond_trusted);
		final boolean [] trusted_weak = this.cond_trusted; // or even any?

		final Thread[] threads = ImageDtt.newThreadArray(biCamDSI.threadsMax);
		final AtomicInteger ai = new AtomicInteger(0);
		target_disparity = new double [num_tiles];
		for (int i = 0; i < num_tiles; i++) target_disparity[i] = Double.NaN;
		ai.set(0);
		final AtomicInteger num_new = new AtomicInteger(0);
		// find definitely trusted and conditionally trusted tiles
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				@Override
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) if (discard_strong || !trusted_sw[nTile]){
						if (nTile == 52681) {
							System.out.println("suggestNewScan(), nTIle="+nTile+", dxy={"+dxy[0]+","+dxy[1]+"}");

						}
						int nTile1 =  tnImage.getNeibIndex(nTile, dxy[0], dxy[1]);
//						if ((nTile1 >= 0) && trusted_weak[nTile1]) { // weak trusted OK, maybe even any measured
						if ((nTile1 >= 0) && (ds[0][nTile1] > 0)) { // weak trusted OK, maybe even any measured
							double new_disp = ds[0][nTile1];
							if (Math.abs(new_disp - ds[0][nTile]) < new_diff) { // suggested is too close to already measured
								continue; // already measured for this tile
							}
							// compare to the previous suggestion
							int previous_indx = ((src_index[nTile] < 0)? list_index:src_index[nTile]) -1;
							double previous_target = (previous_indx >= 0)? biCamDSI.getBiScan(previous_indx).target_disparity[nTile]:Double.NaN; // Nothing is known about the target of the 0-scan
							if (Math.abs(new_disp - previous_target) < new_diff) { // suggested is too close to already suggested and result - disabled
								continue; // already measured for this tile
							}
							// see if close one was already disabled
							boolean valid_suggestion = true;
							for (BiScan other_scan:biCamDSI.biScans) if ( (other_scan.list_index <= list_index) && (other_scan.disabled_measurement[nTile] || remove_all_tried)) {
//								int other_indx = (other_scan.src_index[nTile] < 0)? other_scan.list_index:other_scan.src_index[nTile];
//								double other_disparity = biCamDSI.getBiScan(other_indx).disparity_measured[nTile];
								double other_disparity = other_scan.disparity_measured[nTile];
								if (Math.abs(new_disp - other_disparity) < new_diff) { // suggested is too close to already measured and disabled
									valid_suggestion = false;
									break; // already measured for this tile
								}
								int other_indx = other_scan.list_index;
								double other_target = (other_indx > 0)? biCamDSI.getBiScan(other_indx - 1).target_disparity[nTile]:Double.NaN; // Nothing is known about the target of the 0-scan
								if (Math.abs(new_disp - other_target) < new_diff) { // suggested is too close to already suggested and result - disabled
									valid_suggestion = false;
									break; // already measured for this tile
								}
							}
							if (valid_suggestion) {
								target_disparity[nTile] = new_disp;
								num_new.getAndIncrement();
							}
						}
					}
				}
			};
		}
		ImageDtt.startAndJoin(threads);
		return num_new.get();
	}






	/**
	 * Calculate trusted tiles from the strength and disparity. Trusted are tiles that are really strong
	 * (then they are trusted regardless of neighbors) or are somewhat strong and have sufficient neighbors
	 * that (together with this tile) make a good (tilted) plane
	 * @param trusted_strength strength to trust unconditionally
	 * @param strength_rfloor strength floor to subrtact as a fraction of the trusted strength
	 * @param cond_rtrusted fraction of the trusted strength (after subtracting str4ength_floor) that is sufficient
	 * to participate in plane fitting, if successful - make a tile trusted
	 * @param strength_pow raise strength to thyis power (normally just 1.0)
	 * @param smpl_radius sample "radius", square side is  2 * smpl_radius + 1
	 * @param smpl_num minimal absolute number of samples required to try fit a plane and validate a tile
	 * @param smpl_fract minimal fraction number of the neighbor samples that fit the rms filter required to try fit a plane and validate a tile
	 * @param max_adiff maximal absolute difference from the center tile for initial neighbors selection
	 * @param max_rdiff maximal (additional) relative (to tile disparity) difference from the center tile for initial neighbors selection
	 * @param max_atilt maximal absolute tilt (pix/tile) for the tilted planes to fit
	 * @param max_rtilt maximal relative tilt (pix/tile per disparity pixel). min(max_rtilt*disparity, max_atilt) will be used
	 * @param smpl_arms maximal absolute rms of the weighted remaining samples for the successful plane fitting
	 * @param smpl_rrms maximal relative (additional)rms of the weighted remaining samples for the successful plane fitting
	 * @param damp_tilt regularization value to handle planes if the remaining samples are co-linear (or just a single tile)
	 * @param rwsigma weight Gaussina sigma to reduce influence of far tiles relative to smpl_radius
	 * @param dbg_x tileX to debug
	 * @param dbg_y tileY to debug
	 * @param debugLevel debug level
	 * @return array of 3 numers: number of trusted strong tiles, number of additional trusted by plane fitting, and number of all
	 * somewhat strong tiles
	 */


	int [] calcTrusted(
		    final double     trusted_strength, // trusted correlation strength
			final double     strength_rfloor,   // strength floor - relative to trusted
		    final double     cond_rtrusted,     // minimal strength to consider - fraction of trusted
			final double     strength_pow,      // raise strength-floor to this power
			final int        smpl_radius,
			final int        smpl_num,   //         = 3;      // Number after removing worst (should be >1)
			final double     smpl_fract, // Number of friends among all neighbors
			final double     max_adiff,  // Maximal absolute difference between the center tile and friends
			final double     max_rdiff, //  Maximal relative difference between the center tile and friends
			final double     max_atilt, //  = 2.0; // pix per tile
			final double     max_rtilt, //  = 0.2; // (pix / disparity) per tile
			final double     smpl_arms, //         = 0.1;    // Maximal RMS of the remaining tiles in a sample
			final double     smpl_rrms,        //      = 0.005;  // Maximal RMS/disparity in addition to smplRms
			final double     damp_tilt, //   =     0.001; // Tilt cost for damping insufficient plane data
			final double     rwsigma,           //  = 0.7; // influence of far neighbors diminish as a Gaussian with this sigma
			final int        dbg_x,
			final int        dbg_y,
			final int        debugLevel

			) {
		final double goal_fraction_rms = 0.5;
		final TileNeibs  tnImage = biCamDSI.tnImage;
		final double     wsigma = rwsigma*smpl_radius;
		final double [][] ds = getDisparityStrength( // already has disabled zeroed
	    		false,   // final boolean only_strong,
	    		false,   // final boolean only_trusted,
	    		true) ;  // final boolean only_enabled);
		final int num_tiles = tnImage.getSizeX()*tnImage.getSizeY();
		final double     strength_floor = trusted_strength * strength_rfloor;
		final double     min_strength = strength_floor + (trusted_strength - strength_floor) *  cond_rtrusted;
		final int        smpl_side = 2 * smpl_radius + 1; // Sample size (side of a square)
		final int        smpl_len = smpl_side*smpl_side;
		// prepare window
		final double [][] weights = new double [smpl_radius + 1][smpl_radius + 1];
		for (int i = 0; i <weights.length; i++) {
			for (int j = i; j <weights[i].length; j++) {
				weights[i][j] = (wsigma >0.0) ?Math.exp(-(i*i+j*j)/(2*wsigma*wsigma)):1.0;
				weights[j][i] = weights[i][j];
			}
		}
		final boolean [][] halves = new boolean [8][smpl_len];
		{
			for (int i = 0; i < smpl_side; i++) {
				for (int j = 0; j < smpl_side; j++) {
					int indxs = i*smpl_side+ j;
					halves[0][indxs] = (i <= smpl_radius);
					halves[4][indxs] = (i >= smpl_radius);
					halves[2][indxs] = (j >= smpl_radius);
					halves[6][indxs] = (j <= smpl_radius);
					halves[1][indxs] = (j >= i);
					halves[5][indxs] = (j <= i);
					halves[3][indxs] = ((i + j) >= (2 * smpl_radius));
					halves[7][indxs] = ((i + j) <= (2 * smpl_radius));
				}
			}
		}



		final Thread[] threads = ImageDtt.newThreadArray(biCamDSI.threadsMax);
		final AtomicInteger ai = new AtomicInteger(0);
		final AtomicInteger num_non_weak = new AtomicInteger(0);
		final AtomicInteger num_trusted_strong = new AtomicInteger(0);
		final AtomicInteger num_trusted_plane = new AtomicInteger(0);
//		final boolean [] cond_trusted = new boolean [num_tiles];
		strong_trusted = new boolean [num_tiles];
		trusted =        new boolean [num_tiles];
		cond_trusted =   new boolean [num_tiles];

		ai.set(0);
		// find definitely trusted and conditionally trusted tiles
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				@Override
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()){
						if (!Double.isNaN(ds[0][nTile]) && (ds[1][nTile] >= min_strength) || (ds[1][nTile] > 0)) {
							num_non_weak.getAndIncrement();
							cond_trusted[nTile] = true;
							if (ds[1][nTile] > trusted_strength) {
								strong_trusted[nTile] = true;
								trusted[nTile] =        true;
								num_trusted_strong.getAndIncrement();
							}
						}
					}
				}
			};
		}
		ImageDtt.startAndJoin(threads);

		ai.set(0);
		// find definitely trusted and conditionally trusted tiles
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				@Override
				public void run() {
					int smpl_center = (smpl_side + 1) * smpl_radius;
					for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) if (cond_trusted[nTile] && !trusted[nTile]){
						double [] smpl_d =    new double  [smpl_len];
						double [] smpl_w =    new double  [smpl_len];
						double [] smpl_p =    new double  [smpl_len]; // plane disparity,
						boolean [] smpl_trusted = new boolean [smpl_len];
						// copy neighbor tiles
						double disp_center = ds[0][nTile];
						double max_diff = max_adiff + max_rdiff * disp_center;
						double max_tilt = max_rtilt * disp_center;
						if (max_tilt > max_atilt) {
							max_tilt = max_atilt;
						}

						int nsmpls = 0;
						int nall = 0;

						for (int dy = -smpl_radius; dy <= smpl_radius; dy++) {
							int ady = (dy > 0)? dy:(-dy);
							for (int dx = -smpl_radius; dx <= smpl_radius; dx++) {
								int nTile1 =  tnImage.getNeibIndex(nTile, dx, dy);
								if ((nTile1 >= 0) && cond_trusted[nTile1]) {
									int adx = (dx > 0)? dx:(-dx);
									double max_fdiff = max_diff + (ady+adx) * max_tilt;
									int smpl_indx = smpl_center + dy*smpl_side + dx;
									smpl_trusted[smpl_indx] = true;
									nall++;
									if (Math.abs(ds[0][nTile1] - disp_center) <= max_fdiff) {
//										smpl_sel[smpl_indx] = true;
										smpl_d[smpl_indx] = ds[0][nTile1];
										smpl_w[smpl_indx] = (ds[1][nTile1]-strength_floor) * weights[ady][adx];
										if (strength_pow != 1.0) {
											smpl_w[smpl_indx] = Math.pow(smpl_w[smpl_indx], strength_pow);
										}
										nsmpls ++;
									}
								}
							}
						}
						int fin_samples= (int) ( nall * smpl_fract);
						if (fin_samples < smpl_num) fin_samples = smpl_num;
						double max_rms = smpl_arms + smpl_rrms * disp_center;
						if (nsmpls >= fin_samples) {
							double [] smpl_w_persistent = smpl_w.clone();
							double [] fit_rslt = fitPlaneRemoveOutliers(
									smpl_radius, // int                     smpl_radius,
									max_tilt,    // double                  max_tilt,
									damp_tilt,   // double                  damp_tilt, //   =     0.001; // Tilt cost for damping insufficient plane data
									false,       // boolean                 full_plane,
									smpl_d,      // double []               smpl_d,
									smpl_w,      // double []               smpl_w, // will be modified,
									smpl_p,      // double []               smpl_p, // will be set if provided
									fin_samples, // int                     fin_samples, // remove until this number remain
									goal_fraction_rms*max_rms,    // double                  fin_rms,
									true,        // boolean                 keep_center, // do not remove center tile - it is the tile that should be verified by neighbors
									0,           // int                     fourq_min,         // each of the 4 corners should have at least this number of tiles.
									null,        // int [] 	                fourq_corner, //  array specifying corner number (0..3), -1 - gap. null when not used
									debugLevel); // int                     debugLevel)
//							if ( (fit_rslt != null) && (fit_rslt[0] < (smpl_arms + smpl_rrms * fit_rslt[1]))){
							if ( (fit_rslt != null) && (fit_rslt[0] < max_rms)){
								// Trusted tile, save it
								trusted[nTile] = true;
								num_trusted_plane.getAndIncrement();
								continue;
							}
							// try 8 "halves" around the tile - it may be weak background close to the strong foreground (or stray FG tile)
							for (int dir = 0; dir < halves.length; dir++) {
								nsmpls = 0;
								nall = 0;
								smpl_w = new double [smpl_len];
								for (int indxs = 0; indxs < smpl_len; indxs++) if (halves[dir][indxs]) {
									if (smpl_trusted[indxs]) nall++;
									if (smpl_w_persistent[indxs] > 0.0) {
										smpl_w[indxs] = smpl_w_persistent[indxs];
										nsmpls++;
									}
								}
								fin_samples= (int) ( nall * smpl_fract);
								if (fin_samples < smpl_num) fin_samples = smpl_num;
								if (nsmpls >= fin_samples) {
									fit_rslt = fitPlaneRemoveOutliers(
											smpl_radius, // int                     smpl_radius,
											max_tilt,    // double                  max_tilt,
											damp_tilt,   // double                  damp_tilt, //   =     0.001; // Tilt cost for damping insufficient plane data
											false,       // boolean                 full_plane,
											smpl_d,      // double []               smpl_d,
											smpl_w,      // double []               smpl_w, // will be modified,
											smpl_p,      // double []               smpl_p, // will be set if provided
											fin_samples, // int                     fin_samples, // remove until this number remain
											goal_fraction_rms*max_rms,    // double                  fin_rms,
											true,        // boolean                 keep_center, // do not remove center tile - it is the tile that should be verified by neighbors
											0,           // int                     fourq_min,         // each of the 4 corners should have at least this number of tiles.
											null,        // int [] 	                fourq_corner, //  array specifying corner number (0..3), -1 - gap. null when not used
											debugLevel); // int                     debugLevel)
									if ((fit_rslt != null) && (fit_rslt[0] < max_rms)){
										// Trusted tile, save it
										trusted[nTile] = true;
										num_trusted_plane.getAndIncrement();
										break; // from for (int dir = 0; dir < halves.length; dir++) {
									}
								}
							}
						}
					}
				}
			};
		}
		ImageDtt.startAndJoin(threads);
		int [] numTrustedAll = {num_trusted_strong.get(), num_trusted_plane.get(), num_non_weak.get()};
		return numTrustedAll;
	}

	/**
	 * Remove stray tiles that are closer than closest neighbor and weaker than it or
	 *  trusted_strength * min_rstrength
	 * @param trusted_strength absolute raw strength to trust
	 * @param min_rstrength minimal strength to allow lone FG, relative to trusted_strength
	 * @param max_disp_inc maximal disparity difference between this tile and the nearest neighbor
	 * @param dbg_x
	 * @param dbg_y
	 * @param debugLevel
	 * @return number of disabled tiles
	 */

	int  trimWeakLoneFG(
		    final double     trusted_strength, // trusted correlation strength
			final double     min_rstrength,   // strength floor - relative to trusted
			final double     max_disp_inc,
			final int        dbg_x,
			final int        dbg_y,
			final int        debugLevel
			) {
		final AtomicInteger ai_trimmed = new AtomicInteger(0);
		final double min_strength = trusted_strength * min_rstrength;
		final TileNeibs  tnImage = biCamDSI.tnImage;
		final int num_tiles = tnImage.sizeX * tnImage.sizeY;
		final Thread[] threads = ImageDtt.newThreadArray(biCamDSI.threadsMax);
		final AtomicInteger ai = new AtomicInteger(0);
		final double [][] ds = getDisparityStrength( // already has disabled zeroed
	    		false,   // final boolean only_strong,
	    		false,   // final boolean only_trusted,
	    		true) ;  // final boolean only_enabled);

		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				@Override
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) if (!Double.isNaN(ds[0][nTile])){
						double max_disp = 0;
						double max_disp_w = 0;
						double d_lim = ds[0][nTile] - max_disp_inc;
						double w =     ds[1][nTile];
						for (int dir = 0; dir < 8; dir++) {
							int nTile1 = tnImage.getNeibIndex(nTile, dir);
							if ((nTile1 >=0) && (ds[0][nTile1] > max_disp)){
								 max_disp =   ds[0][nTile1];
								 max_disp_w = ds[1][nTile1];
							}
							if (max_disp > d_lim) {
								break;
							}
						}
						if ((max_disp <= d_lim) && ((w < max_disp_w) || (w < min_strength))) {
							disableTile(nTile);
							ai_trimmed.getAndIncrement();
							if (debugLevel > -1) {
								System.out.println("trimWeakLoneFG: removing tile "+nTile+" ("+(nTile%tnImage.sizeX)+":"+(nTile/tnImage.sizeX)+")");
							}
						}
					}
				}
			};
		}
		ImageDtt.startAndJoin(threads);
		return ai_trimmed.get();
	}

	// FG edge should be strong
	// Trimming(disabling) weak (trusted but not strong_trusted) tiles if on any one side:
	// a) there are no same plane or closer tiles
	// b) there are no in-plane or closer strong tiles, but there are some (strong or any?) farther tiles
	// repeat while more are trimmed
	// maybe, if there are both strong in-plane and far - see which are closer

	/**
	 * Disable low-textured tiles are not between strong tiles, but on one side of it.
	 * This method relies on the assumption that FG edge should bave strong correlation, so it tries multiple directions
	 * from the weak (not trusted strong) tiles and trims tiles that eithre do not have anything in that direction or have
	 * farther tiles.
	 * Trimming(disabling) weak (trusted but not strong_trusted) tiles if on any one side:
	 *   a) there are no same plane or closer tiles
	 *   b) there are no in-plane or closer strong tiles, but there are some (strong or any?) farther tiles
	 * repeat while more are trimmed
	 * maybe, if there are both strong in-plane and far - see which are closer
	 * @param trusted_strength strength to trust unconditionally
	 * @param strength_rfloor strength floor to subrtact as a fraction of the trusted strength
	 * @param cond_rtrusted fraction of the trusted strength (after subtracting str4ength_floor) that is sufficient
	 * to participate in plane fitting, if successful - make a tile trusted
	 * @param strength_pow raise strength to thyis power (normally just 1.0)
	 * @param smpl_radius sample "radius", square side is  2 * smpl_radius + 1
	 * @param smpl_num minimal absolute number of samples required to try fit a plane and validate a tile
	 * @param smpl_fract minimal fraction number of the neighbor samples that fit the rms filter required to try fit a plane and validate a tile
	 * @param max_adiff maximal absolute difference from the center tile for initial neighbors selection
	 * @param max_rdiff maximal (additional) relative (to tile disparity) difference from the center tile for initial neighbors selection
	 * @param max_atilt maximal absolute tilt (pix/tile) for the tilted planes to fit
	 * @param max_rtilt maximal relative tilt (pix/tile per disparity pixel). min(max_rtilt*disparity, max_atilt) will be used
	 * @param smpl_arms maximal absolute rms of the weighted remaining samples for the successful plane fitting
	 * @param smpl_rrms maximal relative (additional)rms of the weighted remaining samples for the successful plane fitting
	 * @param damp_tilt regularization value to handle planes if the remaining samples are co-linear (or just a single tile)
	 * @param rwsigma weight Gaussina sigma to reduce influence of far tiles relative to smpl_radius
	 * @param atolerance absolute disparity tolerance to what to consider "far"
	 * @param rtolerance relative to disparity tolerance to what to consider "far"
	 * @param num_dirs number of directions per 2*PI to try
	 * @param blind_dist when trying directions require distance in that direction to exceed this value to count
	 * @param strong_only_far true: require far tiles to be trusted_strong, false - just trusted is OK
	 * @param num_strong_far number of strong trusted far tiles to make this FG tile hanging
	 * @param num_weak_far number of strong trusted far tiles to make this FG tile hanging
	 * @param dbg_x tileX to debug
	 * @param dbg_y tileY to debug
	 * @param debugLevel debug level
	 * @return
	 */

	int  trimWeakFG(
		    final double     trusted_strength, // trusted correlation strength
			final double     strength_rfloor,   // strength floor - relative to trusted
		    final double     cond_rtrusted,     // minimal strength to consider - fraction of trusted
			final double     strength_pow,      // raise strength-floor to this power
			final int        smpl_radius,
			final int        smpl_num,   //         = 3;      // Number after removing worst (should be >1)
			final double     smpl_fract, // Number of friends among all neighbors
			final double     max_adiff,  // Maximal absolute difference betweenthe center tile and friends
			final double     max_rdiff, //  Maximal relative difference between the center tile and friends
			final double     max_atilt, //  = 2.0; // pix per tile
			final double     max_rtilt, //  = 0.2; // (pix / disparity) per tile
			final double     smpl_arms, //         = 0.1;    // Maximal RMS of the remaining tiles in a sample
			final double     smpl_rrms,        //      = 0.005;  // Maximal RMS/disparity in addition to smplRms
			final double     damp_tilt, //   =     0.001; // Tilt cost for damping insufficient plane data
			final double     rwsigma,           //  = 0.7; // influence of far neighbors diminish as a Gaussian with this sigma

			final double     atolerance,  // When deciding closer/farther
			final double     rtolerance,  // same, scaled with disparity
			final int        num_dirs,    // number of directions to try
			final double     blind_dist,  // analyze only tiles farther than this in the selected direction
			final boolean    strong_only_far, // in variant b) only compare with strong far
			final int        num_strong_far,    // number of directions to try
			final int        num_weak_far,     // number of directions to try

			final int        dbg_x,
			final int        dbg_y,
			final int        debugLevel

			) {
		final double goal_fraction_rms = 0.5;
		final TileNeibs  tnImage = biCamDSI.tnImage;
		final int dbg_tile = (debugLevel>-2)?(dbg_x + tnImage.sizeX*dbg_y):-1;
		final double     wsigma = rwsigma*smpl_radius;
		final double [][] ds = getDisparityStrength( // already has disabled zeroed
	    		false,   // final boolean only_strong,
	    		false,   // final boolean only_trusted,
	    		true) ;  // final boolean only_enabled);

		final int num_tiles = tnImage.getSizeX()*tnImage.getSizeY();
		final double     strength_floor = trusted_strength * strength_rfloor;
//		final double     min_strength = strength_floor + (trusted_strength - strength_floor) *  cond_rtrusted;
		final int        smpl_side = 2 * smpl_radius + 1; // Sample size (side of a square)
		final int        smpl_len = smpl_side*smpl_side;
		// prepare window
		final double [][] weights = new double [smpl_radius + 1][smpl_radius + 1];
		for (int i = 0; i <weights.length; i++) {
			for (int j = i; j <weights[i].length; j++) {
				weights[i][j] = (wsigma >0.0) ?Math.exp(-(i*i+j*j)/(2*wsigma*wsigma)):1.0;
				weights[j][i] = weights[i][j];
			}
		}
		final Thread[] threads = ImageDtt.newThreadArray(biCamDSI.threadsMax);
		final AtomicInteger ai = new AtomicInteger(0);
		final AtomicInteger num_disabled = new AtomicInteger(0);
		int total_disabled = 0;
        int npass=0;
		while (true) {
			num_disabled.set(0);
			final boolean [] new_disabled = new boolean[num_tiles]; // this.disabled.clone();
			ai.set(0);
			// find definitely trusted and conditionally trusted tiles
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					@Override
					public void run() {
						int smpl_center = (smpl_side + 1) * smpl_radius;
						for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) if (trusted[nTile] && !strong_trusted[nTile]){
							boolean dbg_this = (dbg_tile == nTile);
							if (dbg_this) {
								System.out.println("trimWeakFG(): debugging tile"+nTile);
							}
							double [] smpl_d =       new double  [smpl_len];
							double [] smpl_w =       new double  [smpl_len];
							double [] smpl_w_all =   new double  [smpl_len];
							double [] smpl_p =       new double  [smpl_len]; // plane disparity,
							boolean [] smpl_strong = new boolean[smpl_len];
							// copy neighbor tiles
							double disp_center = ds[0][nTile];
							double max_diff = max_adiff + max_rdiff * disp_center;
							double max_tilt = max_rtilt * disp_center;
							if (max_tilt > max_atilt) {
								max_tilt = max_atilt;
							}

							int nsmpls = 0;
							int nall = 0;

							for (int dy = -smpl_radius; dy <= smpl_radius; dy++) {
								int ady = (dy > 0)? dy:(-dy);
								for (int dx = -smpl_radius; dx <= smpl_radius; dx++) {
									int nTile1 =  tnImage.getNeibIndex(nTile, dx, dy);
									//								if ((nTile1 >= 0) && cond_trusted[nTile1]) {
									if ((nTile1 >= 0) && trusted[nTile1]) {

										nall++;
										int adx = (dx > 0)? dx:(-dx);
										double max_fdiff = max_diff + (ady+adx) * max_tilt;
										int smpl_indx = smpl_center + dy*smpl_side + dx;
										smpl_d[smpl_indx] = ds[0][nTile1];
										double w = (ds[1][nTile1]-strength_floor) * weights[ady][adx];
										if (smpl_d[smpl_indx] < 0.0) { // discard stray negative disparity
											w = 0.0;
										}
										if (strength_pow != 1.0) {
											w = Math.pow(w, strength_pow);
										}
										smpl_w_all[smpl_indx] = w;
										if (Math.abs(ds[0][nTile1] - disp_center) <= max_fdiff) {
											smpl_w[smpl_indx] = w;
											nsmpls ++;
										}
										smpl_strong[smpl_indx]= strong_trusted[nTile1];
									}
								}
							}
							int fin_samples= (int) ( nall * smpl_fract);
							if (fin_samples < smpl_num) fin_samples = smpl_num;
							double max_rms = smpl_arms + smpl_rrms * disp_center;
//		final double goal_fraction_rms = 0.5;

							if (nsmpls >= fin_samples) {
								double [] fit_rslt = fitPlaneRemoveOutliers(
										smpl_radius, // int                     smpl_radius,
										max_tilt,    // double                  max_tilt,
										damp_tilt,   // double                  damp_tilt, //   =     0.001; // Tilt cost for damping insufficient plane data
										true,       // boolean                 full_plane,
										smpl_d,      // double []               smpl_d,
										smpl_w,      // double []               smpl_w, // will be modified,
										smpl_p,      // double []               smpl_p, // will be set if provided
										fin_samples, // int                     fin_samples, // remove until this number remain
										goal_fraction_rms*max_rms,     // double                  fin_rms,
										true,        // boolean                 keep_center, // do not remove center tile - it is the tile that should be verified by neighbors
										0,           // int                     fourq_min,         // each of the 4 corners should have at least this number of tiles.
										null,        // int [] 	               fourq_corner, //  array specifying corner number (0..3), -1 - gap. null when not used
										debugLevel); // int                     debugLevel)
//								if ( (fit_rslt != null) && (fit_rslt[0] < (smpl_arms + smpl_rrms * fit_rslt[1]))){
								if ( (fit_rslt != null) && (fit_rslt[0] < max_rms)){
									// check directions here, use smpl_w_all, smpl_d and smpl_p
									//								int [] num_this_closer =        new int [num_dirs];
									//								int [] num_this_closer_strong = new int [num_dirs];
									//								int [] num_farther =            new int [num_dirs];



									double thershold_disp = -(atolerance + rtolerance*fit_rslt[1]);
									double [][] dbg_data = dbg_this?(new double [5+num_dirs][smpl_len]):null;
									String [] dbg_titles = dbg_this?(new String [5+num_dirs]):null;
									if (dbg_this) {
										for (int indxs = 0; indxs < smpl_len; indxs++) {
											dbg_data[0][indxs] = smpl_d[indxs];
											dbg_data[1][indxs] = smpl_strong[indxs]? smpl_d[indxs]: Double.NaN;
											dbg_data[2][indxs] = smpl_p[indxs];
											dbg_data[3][indxs] = smpl_w_all[indxs];
											dbg_data[4][indxs] = smpl_d[indxs]-smpl_p[indxs];
										}
										dbg_titles[0]= "smpl_d";
										dbg_titles[1]= "smpl_d-strong";
										dbg_titles[2]= "smpl_p";
										dbg_titles[3]= "smpl_w_all";
										dbg_titles[4]= "smpl_d-smpl_p";
										for (int idir = 0; idir<num_dirs; idir++) {
											dbg_titles[5 + idir]="dir-"+idir;
											for (int indxs = 0; indxs < smpl_len; indxs++) {
												dbg_data[5+idir][indxs] = Double.NaN;
											}
										}
									}

									for (int idir = 0; idir<num_dirs; idir++) {
										double a = 2*Math.PI*idir/num_dirs;
										double ca = Math.cos(a);
										double sa = Math.sin(a);
										int num_this_closer =        0;
										int num_this_closer_strong = 0;
										int num_farther =            0;
										int num_farther_strong =     0;


										for (int indxs = 0; indxs < smpl_len; indxs++) if (smpl_w_all[indxs] > 0.0) {
											int dy = indxs/smpl_side - smpl_radius;
											int dx = indxs%smpl_side - smpl_radius;
											double d = dx*ca + dy*sa;
											if (dbg_this) {
												dbg_data[5+idir][indxs] = d - blind_dist;
											}
											if (d >= blind_dist) {
												boolean farther = (smpl_d[indxs]-smpl_p[indxs]) < thershold_disp;
												if (farther) {
													num_farther ++;
//													if (!strong_only_far || smpl_strong[indxs]) num_farther++;
													if (smpl_strong[indxs]) num_farther_strong++;
												} else {
													num_this_closer++;
													if (smpl_strong[indxs]) num_this_closer_strong++;
												}
											}

										}
										if (dbg_this) {
											System.out.println("trimWeakFG(): idir="+idir+" num_this_closer="+num_this_closer+
													", num_this_closer_strong="+num_this_closer_strong+", num_farther="+num_farther);
										}
										boolean far_exist = (num_farther_strong > num_strong_far) || (!strong_only_far && (num_farther > num_weak_far));

										if ((num_this_closer ==0) || far_exist) {
											new_disabled[nTile] =    true;
											num_disabled.getAndIncrement();
											if (dbg_this) {
												System.out.println("trimWeakFG(): DISABLED idir="+idir+" num_this_closer="+num_this_closer+
														", num_this_closer_strong="+num_this_closer_strong+", num_farther="+num_farther+", num_farther_strong="+num_farther_strong);
											}
											if (!dbg_this) {
												break; // already disabled
											}

										}
									}
									if (dbg_this) {
									ShowDoubleFloatArrays.showArrays(
											dbg_data,
											smpl_side,
											smpl_side,
											true,
											"trimWeakFG_T"+nTile,
											dbg_titles);
									}


								} else { // may get even after initial filtering (by calcTrusted()) here because some neighbors became disabled? loosen flatness? Keep non-flat
									if (dbg_this) {
										System.out.println("trimWeakFG(): Too high RMS");
									}

								}
							} else { // may get even after initial filtering (by calcTrusted()) here because some neighbors became disabled?
								if (dbg_this) {
									System.out.println("trimWeakFG(): Insufficient points");
								}
							}
						}
					}
				};
			}
			ImageDtt.startAndJoin(threads);
			npass++;
			if (debugLevel > -2) {
				System.out.println("trimWeakFG(): npass="+npass+" removed="+num_disabled.get());
			}
			if (num_disabled.get() == 0) {
				break;
			}
			ai.set(0);
			// find definitely trusted and conditionally trusted tiles
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					@Override
					public void run() {
						for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement())	if (new_disabled[nTile]) {
							disableTile(nTile); // disabled source and this trusted*
						}
					}
				};
			}
			ImageDtt.startAndJoin(threads);
			if (debugLevel > -2) {
				biCamDSI.getLastBiScan(BISCAN_ANY).showScan(
						"trimWeakFG_"+npass);
			}

			total_disabled += num_disabled.get();
		}
		return total_disabled; // numTrustedAll;
	}



//					smpl_p[indxs] = approx2d[0][0] * (sx - smpl_radius) + approx2d[0][1] * (sy - smpl_radius) + approx2d[0][2];

	/**
	 * Remove worst outliers when fitting planes until the specified number of samples are left, re-fitting
	 * plane after each iteration.
	 * Additional mode for filling gaps (not extending) - "four quadrants" is activated when  fourq_min > 0
	 * In this mode each of the four corners of the sample square (after removing center 2*fourq_gap -1 rows
	 * and columns should have at least fourq_min remaining tiles
	 * @param smpl_radius sample "radius", square side is  2 * smpl_radius + 1
	 * @param max_tilt maximal DSI plane tilt (pixels/tile)
	 * @param damp_tilt tilt cost for damping insufficient plane data
	 * @param full_plane generate full tilted plane, not only for non-zero tiles
	 * @param smpl_d  array of disparity values to approximate
	 * @param smpl_w  weights - only >0.0 are processed
	 * @param smpl_p approximated disparity values, may be null. When provided (not null), will have calculated disparity approximation
	 * @param fin_samples remove until this number of tiles remain
	 * @param fin_rms - OK RMS - exit if it is below (set to 0.0 if unknown yet)
	 * @param keep_center do not remove center tile - it is the tile that should be verified by neighbors
	 * @param fourq_min each of the 4 corners should have at least this number of tiles.
	 * @param fourq_corner array specifying corner number (0..3), -1 - gap. null when not used
	 * @param debugLevel debug level
	 * @return array of 4 values: rms of the remaining samples,  average (center) disparity value, tiltX, tiltY
	 */
	public double [] fitPlaneRemoveOutliers(
			int                     smpl_radius,
			double                  max_tilt,
			double                  damp_tilt, //   =     0.001; // Tilt cost for damping insufficient plane data
			boolean                 full_plane,
			double []               smpl_d,
			double []               smpl_w, // will be modified,
			double []               smpl_p, // will be set if provided
			int                     fin_samples, // remove until this number remain
			double                  fin_rms,
			boolean                 keep_center, // do not remove center tile - it is the tile that should be verified by neighbors
			int                     fourq_min,
			int []                  fourq_corner,
			int                     debugLevel)
	{
		int [] num_in_corners = ((fourq_corner == null) || (fourq_min == 0)) ? null: new int [5]; // last is not used
		final int keep_tile = keep_center? (2*smpl_radius*(smpl_radius+1)): -1; // do not remove tile with this number
		double [] disp_tilts = null;
		final int        smpl_side = 2 * smpl_radius + 1; // Sample size (side of a square)
		int num_in_sample = 0;
		for (int i = 0; i < smpl_w.length; i++) if (smpl_w[i]> 0.0) {
			num_in_sample++;
			if (num_in_corners != null) num_in_corners[fourq_corner[i]]++;
		}

		int smpl_len = smpl_w.length;
		if (num_in_sample < fin_samples) {
			return null;
		}
		if (num_in_corners != null) {
			for (int i = 0; i < 4; i++) {
				if (num_in_corners[i] < fourq_min) {
					return null;
				}
			}
		}
		double rms = Double.NaN;
		if (smpl_p == null) smpl_p = new double [smpl_len];
		while (num_in_sample >= fin_samples) {
			// fit plane to the selected and remove outliers after each iteration
			disp_tilts = fitPlane(
					smpl_radius,  //int        smpl_radius,
					max_tilt,     //double     max_tilt,
					damp_tilt,    //double     damp_tilt, //   =     0.001; // Tilt cost for damping insufficient plane data
					full_plane,   // boolean                 full_plane,
					smpl_d,       // double []  smpl_d,
					smpl_w,       // double []  smpl_w,
					smpl_p,       //double []  smpl_p, // will be set if provided
					debugLevel);  // int        debugLevel)
			// calculate fitting quality
			if (disp_tilts == null) {
				return null;
			}
//			double d_center = disp_tilts[0];
			double sw = 0.0, sd2 = 0.0;
			for (int indxs = 0; indxs < smpl_len;indxs++) if (smpl_w[indxs] > 0) {
				double d = smpl_d[indxs] - smpl_p[indxs];
				double dw = d * smpl_w[indxs];
				sd2 += dw * smpl_d[indxs];
				sw +=       smpl_w[indxs];
			}
			rms = (sw > 0.0)? Math.sqrt(sd2/sw): Double.NaN;

			// remove worst - it should not make remaining set
			if ((num_in_sample > fin_samples) && !(rms < fin_rms)) { // remove worst if it is not the last run where only calculations are needed
				int iworst = -1;
				double dworst2 = 0.0;
				for (int indxs = 0; indxs < smpl_len; indxs++) if (smpl_w[indxs] > 0.0) {
					if (indxs == keep_tile) { // it was the center tile disabled from removing
						continue;
					}
					// verify that the worst tile will not leave the corner too empty
					int num_corner = 4; // unused, gap
					if (num_in_corners != null) {
						num_corner =  fourq_corner[indxs];
						if ((num_corner < 4) && (num_in_corners[num_corner] <= fourq_min)) {
							continue; // can not remove this tile
						}
					}

					double d2 = smpl_d[indxs] - smpl_p[indxs];
					d2 *=d2;
					if (d2 > dworst2) {
						if ((damp_tilt !=0.0) || notColinearWithout (
								indxs, // int        indx,
								smpl_w, // boolean [] sel,
								smpl_side)) { // int side))
							iworst = indxs;
							dworst2 = d2;
						}
					}
				}
				if (iworst < 0){ // can happen if damp_tilt == 0.0 and will become colinear
					if (debugLevel > 0) {
						System.out.println("**** this may be BUG in fitPlaneRemoveOutliers() can not find the worst sample  - all tiles fit perfectly ****");
					}
					// this can happen if some samples are the same and all the pixels fit exactly - use all of them
					break;
				}
				// remove worst sample
				if (num_in_corners != null) {
					num_in_corners[fourq_corner[iworst]]--; // corner counter - can remove from the gap, but nobody count that
				}
				smpl_w[iworst] = 0.0;
				num_in_sample --;
			} else {
				break;
			}
		} // while (nsmpls >= min_sample) {
		double [] rslt = {rms, disp_tilts[0], disp_tilts[1], disp_tilts[1]};
		return rslt;
	}

	/**
	 * Verify that selected points are not all on the same line, even if the specified one is removed
	 * @param indx index of the point to be removed
	 * @param smpl_w 2-d sample weights in linescan order (used zero/non-zero only)
	 * @param side square samples side
	 * @return true if there are enough samples for plane extraction, false otherwise
	 */

	public boolean notColinearWithout (
			int        indx,
			double [] smpl_w,
			int side)
	{
		if (smpl_w[indx] == 0.0){
			throw new IllegalArgumentException ("notCoplanarWithout(): specified is the non existing index");
		}
		double w = smpl_w[indx];
		smpl_w[indx] = 0.0;
		boolean rslt = notColinear ( smpl_w, side);
		smpl_w[indx] = w; // restore value
		return rslt;
	}
	/**
	 * Verify that selected points are not all on the same line
	 * @param smpl_w 2-d sample weights in linescan order (used zero/non-zero only)
	 * @param side square samples side
	 * @return true if there are enough samples for plane extraction, false otherwise
	 */
	public boolean notColinear (
			double [] smpl_w,
			int side)
	{
		int indx0, indx1;
		for (indx0 = 0; indx0 < smpl_w.length; indx0++){
			if (smpl_w[indx0] > 0.0) break;
		}
		for (indx1 = indx0+1; indx1 < smpl_w.length; indx1++){
			if (smpl_w[indx0] > 0.0) break;
		}
		if (indx1 >= smpl_w.length) return false; // too few points;
		int sx0 = indx0 % side;
		int sy0 = indx0 / side;
		int sx1 = indx1 % side;
		int sy1 = indx1 / side;
		for (int indx = indx1 +1; indx < smpl_w.length; indx++){
			int sx = indx % side;
			int sy = indx / side;
			if ((sx - sx0) * (sy - sy1) != (sx - sx1) * (sy - sy0)){
				return true;
			}
		}
		return false;
	}



	/**
	 * Approximate tile disparity by a tilted DSI plane
	 * @param smpl_radius sample "radius", square side is  2 * smpl_radius + 1
	 * @param max_tilt maximal DSI plane tilt (pixels/tile)
	 * @param damp_tilt tilt cost for damping insufficient plane data
	 * @param full_plane generate full tilted plane, not only for non-zero tiles
	 * @param smpl_d  array of disparity values to approximate
	 * @param smpl_w  weights - only >0.0 are processed
	 * @param smpl_p approximated disparity values, amy be null. When provided (not null), will have calculated disparity approximation
	 * @param debugLevel debug level
	 * @return array of 3 values: average (center) disparity value, tiltX, tiltY
	 */
	public double [] fitPlane(
			int                     smpl_radius,
			double                  max_tilt,
			double                  damp_tilt, //   =     0.001; // Tilt cost for damping insufficient plane data
			boolean                 full_plane,
			double []               smpl_d,
			double []               smpl_w,
			double []               smpl_p, // will be set if provided
			int                     debugLevel)
	{
		PolynomialApproximation pa = new PolynomialApproximation();
		final double [] damping = {damp_tilt, damp_tilt, 0.0}; // 0.0 will be applied to average value, tilt_cost - to both tilts
		final int        smpl_side = 2 * smpl_radius + 1; // Sample size (side of a square)
		final int        smpl_len = smpl_side*smpl_side;
		if (smpl_p == null) smpl_p = new double [smpl_len];
		int num_in_sample = 0;
		for (int i = 0; i < smpl_w.length; i++) if (smpl_w[i] > 0.0) num_in_sample++;

		double [][][] mdata = new double [num_in_sample][3][];
		int mindx = 0;
		for (int sy = 0; sy < smpl_side; sy++){
			for (int sx = 0; sx < smpl_side; sx++){
				int indxs = sy * smpl_side + sx;
				if (smpl_w[indxs] > 0.0) {
					mdata[mindx][0] = new double [2];
					mdata[mindx][0][0] =  sx - smpl_radius;
					mdata[mindx][0][1] =  sy - smpl_radius;
					mdata[mindx][1] = new double [1];
					mdata[mindx][1][0] = smpl_d[indxs];
					mdata[mindx][2] = new double [1];
					mdata[mindx][2][0] =  smpl_w[indxs];
					mindx ++;
				}
			}
		}
		double[][] approx2d = pa.quadraticApproximation(
				mdata,
				true,          // boolean forceLinear,  // use linear approximation
				damping,       // double [] damping,
				THRESHOLD_LIN,  // threshold ratio of matrix determinant to norm for linear approximation (det too low - fail)
				THRESHOLD_QUAD, // threshold ratio of matrix determinant to norm for quadratic approximation (det too low - fail)
				debugLevel);
		if (approx2d == null){
			if (debugLevel > -1){
				System.out.println("getDisparityStrengthML(): can not find linear approximation");
			}
			return null;
		}
		// limit tilt to be within range
		//											double     max_abs_tilt, //  = 2.0; // pix per tile
		//											double     max_rel_tilt, //  = 0.2; // (pix / disparity) per tile
//		double max_tilt = Math.min(mlfp.max_abs_tilt, mlfp.max_rel_tilt * approx2d[0][2]);
		boolean overlimit = (Math.abs(approx2d[0][0]) > max_tilt) || (Math.abs(approx2d[0][1]) > max_tilt);
		if (overlimit) {
			approx2d[0][0] = Math.min(approx2d[0][0],  max_tilt);
			approx2d[0][1] = Math.min(approx2d[0][1],  max_tilt);
			approx2d[0][0] = Math.max(approx2d[0][0], -max_tilt);
			approx2d[0][1] = Math.max(approx2d[0][1], -max_tilt);
		}
		// subtract tilt from disparity
		for (int sy = 0; sy < smpl_side; sy++){
			for (int sx = 0; sx < smpl_side; sx++){
				int indxs = sy * smpl_side + sx;
				if ((smpl_w[indxs] > 0.0) || full_plane) {
					smpl_p[indxs] = approx2d[0][0] * (sx - smpl_radius) + approx2d[0][1] * (sy - smpl_radius) + approx2d[0][2];
				}
			}
		}

		if (overlimit){ // re-calculate disparity average (in the center)
			double sw = 0.0;
			double sd=0.0;
			for (int indxs = 0; indxs < smpl_len;indxs++) if (smpl_w[indxs] > 0.0) {
				double d = smpl_d[indxs] - smpl_p[indxs];
				double dw = d * smpl_w[indxs];
				sd += dw;
				sw += smpl_w[indxs];
			}
			sd /= sw;
			for (int indxs = 0; indxs < smpl_len;indxs++) if ((smpl_w[indxs] > 0.0) || full_plane) {
				smpl_p[indxs] += sd;
			}
			approx2d[0][2] += sd;
		}
		double [] rslt = {approx2d[0][2], approx2d[0][0],approx2d[0][1]}; // {center, tiltX, tiltY
		return rslt;
	}

	public double [] getDensity(
			final boolean strong_only,
			final int need_tiles,
			final int max_radius,
			final int dbg_x,
			final int dbg_y,
			final int debugLevel )
	{
		final int [][][] incr = {
				{{ 1,-1},{ 0, 1}},  // top right corner, going down
				{{ 1, 1},{-1, 0}},  // bottom right corner, going left
				{{-1, 1},{ 0,-1}},  // bottom left corner, going up
				{{-1,-1},{ 1, 0}}}; // top left corner, going right

		final TileNeibs  tnImage = biCamDSI.tnImage;
		final int dbg_tile = (debugLevel > -2)? (dbg_y * tnImage.sizeX + dbg_x):-1;
		final boolean [] en = strong_only ? strong_trusted: trusted;
    	final int num_tiles = en.length;
    	final double [] density = new double[num_tiles];
		final Thread[] threads = ImageDtt.newThreadArray(biCamDSI.threadsMax);
		final AtomicInteger ai = new AtomicInteger(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				@Override
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) {
						if (nTile == dbg_tile) {
							System.out.println("getDensity(): nTile="+nTile);
						}
//						int num_tried = 0, num_found =0;
						int sd = 0, sdr2 = 0, s0=0, sr2 = 0;
						int dx = 0,dy = 0;
						int r = 0;
						label_snail: {
							for (; r <= max_radius; r++) {
								int nd = (r == 0) ? 1: 4;
								int nl = (r == 0) ? 1: (2 * r);
								for (int dir = 0; dir < nd; dir ++) {
									for (int l = 0; l < nl; l++) {
										dx = r * incr[dir][0][0] + l * incr[dir][1][0];
										dy = r * incr[dir][0][1] + l * incr[dir][1][1];
										int nTile1 = tnImage.getNeibIndex(nTile, dx,dy);
										if (nTile1 >= 0) {
											int r2 = dx*dx + dy*dy;
											s0++;
											sr2 += r2;
											if (en[nTile1]) {
												sd++;
												sdr2 += r2;
											}
//											num_found++;
											if (sd >= need_tiles) {
												break label_snail;
											}
										}
									}
								}
							}
						}
						int r02 = 2 * r * r;
						int num = r02 * sd - sdr2;
						int denom = r02 * s0 - sr2;
//						int num = sd;
//						int denom = s0;

						if (denom > 0) {
							density[nTile] = (1.0* num) / denom;
						}
					}
				}
			};
		}
		ImageDtt.startAndJoin(threads);
		return density;
	}

	/**
	 * Select low-textured tiles for averaging measurements
	 * @param min_disparity minimal disparity to accept
	 * @param max_density maximal trusted tile density (density varies from 0.0 to 1.0)
	 * @param grow how many layers of tiles should be added after filling gaps and removing small clusters
	 * @param max_gap_radius maximal radius of a void to be filled
	 * @param min_clust_radius minimal original cluster radius to survive
	 * @param density per-tile values of the density of trusted tiles around it.
	 * @param src_disparity - source disparity array. If null will only use density (that should be > 0)
	 * @return selection of the low-textured tiles to be processed with averaging correlation (3x3 or 5x5 tiles)
	 */
	public boolean [] selectLowTextures(
			double    min_disparity,
			double    max_density,
			int       grow,
			int       max_gap_radius,
			int       min_clust_radius,
			double [] density,
			double [] src_disparity)
	{
		boolean [] selection = new boolean [density.length];
		if (src_disparity == null) {
			for (int nTile = 0; nTile < selection.length; nTile++) {
				if ((density[nTile] <= max_density) && (density[nTile] <= max_density)) { // disparity has NaN-s, they will fail comparisons
					selection[nTile] = true;
				}
			}
		} else {
			for (int nTile = 0; nTile < selection.length; nTile++) {
				if ((src_disparity[nTile] >= min_disparity) && (density[nTile] <= max_density)) { // disparity has NaN-s, they will fail comparisons
					selection[nTile] = true;
				}
			}
		}
		final TileNeibs  tnImage = biCamDSI.tnImage;

		tnImage.growSelection(
				2* max_gap_radius, // int        grow,           // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
				selection, // boolean [] tiles,
				null); // boolean [] prohibit)

		tnImage.shrinkSelection(
				2*(max_gap_radius + min_clust_radius), // int        grow,           // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
				selection, // boolean [] tiles,
				null); // boolean [] prohibit)
		tnImage.growSelection(
				2 * (min_clust_radius + grow), // int        grow,           // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
				selection, // boolean [] tiles,
				null); // boolean [] prohibit)
		return selection;
	}


	/**
	 * Fill the gaps (where provided src_disparity is NaN) and optionally smooth it using pull to the weighted average of 8 neighbors
	 * @param src_disparity source disparity values, some may be undefined (Double.NaN)
	 * @param src_strength optional strengths of the initial values (should be with floor subtracted)
	 * @param selection tile selection to process
	 * @param neib_pull pull of the average neighbor weight relative to the original disparity value of a tile. If 0 - only gaps
	 *  (Double.NaN) values are filled, all the defined disparities remain as they were provided
	 * @param max_iterations Maximal number of the iteration steps
	 * @param min_change exit iterations when the maximal disparity change to a tile is less than this value
	 * @param dbg_x debug tile X coordinate
	 * @param dbg_y debug tile Y coordinate
	 * @param debugLevel debug level
	 * @return processed disparity and optional strength array. Normally only unselected tiles should remain Double.NaN, all other should be interpolated
	 */
	public double [][] fillAndSmooth(
			final double [] src_disparity,
			final double [] src_strength, // if not null will be used for weighted pull
			final boolean [] selection,
			final double     neib_pull, // pull to weighted average relative to pull to the original disparity value. If 0.0 - will only update former NaN-s
			final int max_iterations,
			final double min_change,
			final int dbg_x,
			final int dbg_y,
			final int debugLevel) {
		final double [] weights = {1.0, 0.7}; // {ortho, corners},
		final boolean adjust_all = (neib_pull > 0.0);
//		final double fneib_pull = adjust_all ? neib_pull: 1.0;
		final TileNeibs  tnImage = biCamDSI.tnImage;
//		final int dbg_tile = (debugLevel > -2)? (dbg_y * tnImage.sizeX + dbg_x):-1;
		final int dbg_tile = (debugLevel > 0)? (dbg_y * tnImage.sizeX + dbg_x):-1;
    	final int num_tiles = src_disparity.length;
		final Thread[] threads = ImageDtt.newThreadArray(biCamDSI.threadsMax);
		// max_changes may have Double.NaN value (here meaning larger than any)
		final double [] max_changes = new double [biCamDSI.threadsMax]; // Each thread provides its own maximal change, then they are combined
		final double [] disparity = src_disparity.clone();
		final double [] new_disparity = src_disparity.clone();
		final double [] strength = (src_strength != null)? src_strength: null; // new double [src_disparity.length];
		final double [] new_strength = (src_strength != null)? src_disparity.clone(): null;
//		if (src_strength == null) for (int i = 0; i < strength.length; i++) if (selection[i] && !Double.isNaN(src_disparity[i])) strength[i] = 1.0;
		final AtomicInteger ai = new AtomicInteger(0);
		final AtomicInteger ai_numThread = new AtomicInteger(0);
		for (int num_iter = 0; num_iter < max_iterations; num_iter++) {
			ai.set(0);
			ai_numThread.set(0);
			final AtomicInteger ai_count=new AtomicInteger(0);
			for (int i = 0; i < max_changes.length; i++) max_changes[i] = 0.0;
			final int fnum_iter = num_iter;
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					@Override
					public void run() {
						int numThread = ai_numThread.getAndIncrement(); // unique number of thread to write to max_changes[numThread]
//						max_changes[numThread]
						for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) if (selection[nTile]){
							if (nTile == dbg_tile) {
								System.out.println("fillAndSmooth(): iteration "+fnum_iter+" nTile="+nTile);
							}
							if (!adjust_all && !Double.isNaN(src_disparity[nTile])) {
// nothing to do - new_disparity and new_strength are already same as new_*
							} else {
								double sw=0.0, swd =0.0, sww = 0.0;


								for (int dir = 0; dir < 8; dir++) {
									int nTile1 = tnImage.getNeibIndex(nTile, dir);
									if (nTile1 >= 0) {
										double w = weights[dir & 1];
										double s = (strength == null) ? 1.0 : (Math.max(strength[nTile1], 0.0));
										double d = disparity[nTile1];
										double ww = w * s;
										if ((ww > 0.0) && !Double.isNaN(d)) {
											sw += w;
											sww += ww;
											swd += ww * d;
										}
									}
								}
								if (sww > 0.0) { // then sw
									double d_mean = swd/sww;
									double w_mean = sww/sw;
									if (Double.isNaN(src_disparity[nTile])) {
										new_disparity[nTile] =  d_mean;
										if (strength != null) {
											new_strength[nTile] = w_mean;
										}
									} else {
										double pull_origin = (strength==null)? 1.0: strength[nTile];
										new_disparity[nTile] =  (d_mean *(neib_pull * w_mean) + src_disparity[nTile] * pull_origin)/((neib_pull * w_mean) + pull_origin);
										if (strength != null) {
											new_strength[nTile] =  (w_mean * neib_pull + src_strength[nTile])/(neib_pull + 1);
										}
									}
									ai_count.getAndIncrement();
									double adiff = Math.abs(new_disparity[nTile] - disparity[nTile]); // disparity[nTile] may be NaN then adiff will be NaN as intended
									if (!(adiff < max_changes[numThread])) {
										max_changes[numThread] = adiff; // NaN will be copied
									}
								}
							}
						}
					}
				};
			}
			ImageDtt.startAndJoin(threads);
			System.arraycopy(new_disparity, 0, disparity, 0, num_tiles);
			if (strength != null) {
				System.arraycopy(new_strength, 0, strength, 0, num_tiles);
			}
			double change = 0.0;
			for (int i = 0; i < ai_numThread.get(); i++) {
				if (Double.isNaN(max_changes[i]) || (change < max_changes[i])) { // max_changes[i] may be NaN
					change = max_changes[i];
					if (!(change <= min_change)) { // change may be NaN
						if (debugLevel < -2) {
							// may exit here, continue to get debug info
							break;
						}
					}
				}
			}
			if (debugLevel > 0) {
				System.out.println("fillAndSmooth(): iteration "+fnum_iter+" change="+change+" (min_change="+min_change+")+ tiles updated="+ai_count.get());
			}
			if (change <= min_change) { // change may be NaN
				break; // from the main loop
			}
		}
		double [][] ds = {disparity,strength}; // strength may be null
		return ds;
	}
	/**
	 * Extend low texture areas horizontally if both ends are OK: either belong to the low texture areas selection (lt_sel)
	 * or are trusted and closer or the same (to the provided tolerance)
	 * @param tolerance strong limit should not have disparity lower by more than tolerance than low textured area
	 * @param ds_lt disparity/strength for the low textured area
	 * @param d_single disparity measured for the single-tile correlation
	 * @param lt_sel low -textured selection
	 * @param exp_sel expanded selection (does not intersect with lt_sel
	 * @param trusted trusted tiles selection
	 * @return extended disparity/strength data
	 */
	  public double [][] getLTExpanded(
			  final double      tolerance,
			  final double [][] ds_lt,
			  final double []   d_single,
			  final boolean []  lt_sel, // lt_sel and exp_sel do not intersect
			  final boolean []  exp_sel,
			  final boolean []  trusted)
	  {
//		final int dbg_tile = (debugLevel>-2)?(dbg_x + tnImage.sizeX*dbg_y):-1;
		  final int num_tiles = exp_sel.length;
		  final double [][] ds = new double [2][num_tiles];
		  for (int i = 0; i < num_tiles; i++) ds[0][i] = Double.NaN;
		  final Thread[] threads = ImageDtt.newThreadArray(biCamDSI.threadsMax);
		  final AtomicInteger ai = new AtomicInteger(0);
		  final TileNeibs         tnImage = biCamDSI.tnImage;
		  for (int ithread = 0; ithread < threads.length; ithread++) {
			  threads[ithread] = new Thread() {
				  @Override
				  public void run() {
					  //						max_changes[numThread]
					  for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement())  if (lt_sel[nTile]) {
						  // is low texture = - just copy
						  ds[0][nTile] = ds_lt[0][nTile];
						  ds[1][nTile] = ds_lt[1][nTile];
					  } else if (exp_sel[nTile]) {
						  int dbg_tileX = nTile%tnImage.sizeX;
						  int dbg_tileY = nTile/tnImage.sizeX;
//						  if ((dbg_tileY == 156) || (dbg_tileY == 157)) {
//							  System.out.println("getLTExpanded(): tileX="+dbg_tileX+", tileY="+dbg_tileY);
//							  System.out.println("getLTExpanded(): tileX="+dbg_tileX+", tileY="+dbg_tileY);
//						  }
						  int nTile0= tnImage.getNeibIndex(nTile,-1,0);
						  if ((nTile0 < 0) || !exp_sel[nTile0]){
							  boolean OK0 = (nTile0 < 0) || lt_sel[nTile0] || (trusted[nTile0] && (d_single[nTile0] >= (ds_lt[0][nTile] - tolerance)));
							  if (OK0) {
								  int nTile1= tnImage.getNeibIndex(nTile,1,0);
								  int l = 1;
								  while ((nTile1 >= 0) && exp_sel[nTile1]) {
									  nTile1= tnImage.getNeibIndex(nTile1,1,0);
									  l++;
								  }

								  boolean OK1 = (nTile1 < 0) || lt_sel[nTile1] || (trusted[nTile1] && (d_single[nTile1] >= (ds_lt[0][nTile1-1] - tolerance)));
								  if (OK1) {
									  for (int i = 0; i < l; i++) {
										  int nt = nTile+ i;
										  ds[0][nt] = ds_lt[0][nt];
										  ds[1][nt] = ds_lt[1][nt];
									  }
								  }
							  }
						  }
					  }
				  }
			  };
		  }
		  ImageDtt.startAndJoin(threads);
		  return ds;

	  }


}