package com.elphel.imagej.tileprocessor;

import java.awt.Point;
import java.awt.Rectangle;
import java.text.SimpleDateFormat;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Calendar;
import java.util.concurrent.atomic.AtomicInteger;

import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.cameras.ColorProcParameters;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.gpu.TpTask;
import com.elphel.imagej.tileprocessor.sfm.StructureFromMotion;

public class EstimateSceneRange {
	public static final int TILE_CORNER =        1;
	public static final int TILE_EDGE =          2;
	public static final int TILE_INNER_CORNER =  3;
	public static final int TILE_INNER_EDGE =    4;
	public static final int TILE_STRONG_CORNER = 5;
	public static final int TILE_STRONG_EDGE =   6;
	public static final int TILE_STRONG_INNER =  7;
	public static final int [] MIN_TYPE=   {-1,1,1,3,3,5,5,5};
	public static final int [] INNER_DIST= {-1,0,0,1,1,2,2,3};
//	public static final int [] EDGE_TYPE=  {0,1,0,1,0,1,2};
	public int earliest;
	public int latest;
	public int center;
	public int earliest_prescan;
	public int latest_prescan;
	public boolean needs_prescan = false;
	
	public int getEarliest() {
		return earliest;
	}

	public int getLatest() {
		return latest;
	}

	public int getCenter() {
		return center;
	}

	public void limitRange(int range) {
		int late_lim = center + range/2; // will be symmetrical for odd values
		latest =   Math.min(latest,  late_lim);
		int early_lim = latest - range+1;
		earliest = Math.max(earliest, early_lim);
		earliest_prescan = Math.max(earliest_prescan, earliest);
		latest_prescan =   Math.min(latest_prescan, latest);
		needs_prescan = (earliest_prescan > earliest) || (latest_prescan < latest);
	}
	
	public static EstimateSceneRange getPrescanRange(
			final CLTParameters       clt_parameters,
			final QuadCLT[]           quadCLTs,
			final int                 ref_index,
			final ColorProcParameters colorProcParameters,
			final SetChannels []      set_channels,
			final boolean             batch_mode,
			final int                 debugLevel) {
		if (debugLevel > -3) {
			System.out.println("getPrescanRange(): Will re-evaluate ranges, so new center may be different than"+
		    " early_scene of the current ref_index");
		}
		// Will only limit scan around the center, the center location is the same regardless. It is to keep the same
		// center reference scene (it can still change caused by LY adjustment?)
		boolean lock_position =      clt_parameters.imp.lock_position;
		int tilesX =  quadCLTs[ref_index].getTileProcessor().getTilesX();
		int tile_size = quadCLTs[ref_index].getTileProcessor().getTileSize();
		double  max_meters =        clt_parameters.imp.max_meters;     // 7.5;   // maximal horizontal offset from the reference to use initial SFM (process shorter center first)
		
		double  max_offset =        clt_parameters.imp.max_rel_offset * tilesX * tile_size;
		double  max_roll =          clt_parameters.imp.max_roll_deg*Math.PI/180.0;
		double  max_zoom_diff =     clt_parameters.imp.max_zoom_diff;
		boolean use_quat_corr =     clt_parameters.imp.use_quat_corr;   // use internally (probably deprecated - not)
		boolean inertial_only =     clt_parameters.imp.inertial_only;   // use internally
		boolean fmg_rectilinear =   clt_parameters.imp.fmg_rectilinear;// use rectilinear model for scene offset estimation 
		double  fmg_max_quad =      clt_parameters.imp.fmg_max_quad; // estimate offset by 4 points (rooll-aware, 25% from center) if center
		boolean changed = quadCLTs[ref_index].isPhotometricUpdatedAndReset(); // false was true as it was reset only for reference (last) scene, not for center
		if (debugLevel > -3) {
			System.out.println("getPrescanRange(): Before spawnNoModelQuadCLT() changed = "+changed);
		}
		for (int scene_index =  ref_index - 1; scene_index >= 0 ; scene_index--) {
			// to include ref scene photometric calibration
			// added 05.21.2024: skip if was spawn already
			if (changed || (quadCLTs[scene_index] == null)) { 
				quadCLTs[scene_index] = quadCLTs[ref_index].spawnNoModelQuadCLT( // was resetting dsi!
						set_channels[scene_index].set_name,
						clt_parameters,
						colorProcParameters, //
						Interscene.THREADS_MAX,
						debugLevel-2);
			}
		}
		//			ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection();		
		double avg_z = quadCLTs[ref_index].getAverageZ(true); // use lma
		double max_z_change = Double.NaN; // only applicable for drone images
		if (!lock_position && (max_zoom_diff > 0)) { // ignore if set to
			max_z_change = avg_z * max_zoom_diff;
			if (debugLevel > -3) {
				System.out.println("Average z =  "+ avg_z+" m.");
				System.out.println("Setting maximal Z-direction movement to "+ max_z_change+" m.");
			}
		}
		double [] quat_corr = use_quat_corr? quadCLTs[ref_index].getQuatCorr() : null; //
		double [] enu_corr_metric = quadCLTs[ref_index].getENUCorrMetric();
		// quat_corr may still be null if does not exist

		if (debugLevel > -3) {
			System.out.println("getPrescanRange(): compensate_ims_rotation="+use_quat_corr);
			System.out.println("getPrescanRange(): will "+((quat_corr==null)? "NOT ":"")+"correct orientation offset");
			QuadCLTCPU.showQuatCorr(quat_corr, enu_corr_metric); // enu_corr_metric - linear correction
		}
		double [][][] ims_xyzatr;
		if (inertial_only) {
			ims_xyzatr = QuadCLT.integratePIMU( // does nor use quat_corr
					clt_parameters, // final CLTParameters clt_parameters,
					quadCLTs,       // final QuadCLT[]     quadCLTs,
					quat_corr,      // double []  quat_corr,
					ref_index,      // final int           ref_index,
					null,           // double [][][]       dxyzatr,
					0, // final int           early_index,
					(quadCLTs.length -1), // int           last_index,
					//				    		(new double[2][3]) //final double [][] pimu_offsets // null - will not subtract from velocities
					debugLevel      // final int           debugLevel
					);
		} else {
			ims_xyzatr = quadCLTs[ref_index].getXyzatrIms(
					clt_parameters, // CLTParameters clt_parameters,
					quadCLTs, // QuadCLT[]  quadCLTs,
					quat_corr, // double []  quat_corr, // only applies to rotations - verify!
					debugLevel) ; // int        debugLevel)
		}
		boolean debug_ims_xyzatr = true;
		if (debug_ims_xyzatr) {
			System.out.println("lock_position(set x,y,z = 0)="+lock_position);
			System.out.println(String.format("%6s, %20s, %15s, %15s, %15s, %15s, %15s, %15s", "index","timestamp", "X", "Y", "Z", "Azimuth", "Tilt", "Roll"));
			for (int i = 0; i < quadCLTs.length; i++) {
				System.out.println(String.format("%6d, %20s, %15.12f, %15.12f, %15.12f, %15.12f, %15.12f, %15.12f", i,quadCLTs[i].getImageName(),
						ims_xyzatr[i][0][0],ims_xyzatr[i][0][1],ims_xyzatr[i][0][2],ims_xyzatr[i][1][0],ims_xyzatr[i][1][1],ims_xyzatr[i][1][2]));
			}
		}
		if (lock_position) { // zero linear offsets
			for (int i = 0; i < quadCLTs.length; i++) for (int j = 0; j < 3; j++){
				ims_xyzatr[i][0][j]=0.0;
			}
		}
		// filter by Z, roll, overlap and offset. Find earliest center, then earliest start scene
		int compare_index = ref_index; // first - compare to the last index, whjen found center - compare with it
		int center_index = -1;
		int earliest_index = -1;
		int ealiest_defined = ref_index;
		double [][] inv_compare_xyzatr = ErsCorrection.invertXYZATR(ims_xyzatr[compare_index]);
		// On the first pass, OK to exceed horizontal shift
		boolean horizontal_OK = true; 
		
		
		for (int nscene = ref_index -1; (nscene >= 0) && (quadCLTs[nscene] != null) && (ims_xyzatr[nscene] != null); nscene--) {
			ealiest_defined = nscene;
			double [][] predicted_pose = ErsCorrection.combineXYZATR(ims_xyzatr[nscene], inv_compare_xyzatr);
			check_sequence: { // verify if it is OK to add this scene
				// verify Z
				if (Math.abs(predicted_pose[0][2]) > max_z_change ) {
					if (debugLevel > -3) {
						System.out.println("getPrescanRange(): altitude change exceededs the limit for the scene "+nscene+": "+
								Math.abs(predicted_pose[0][2])+" > "+max_z_change); 
					}
					break check_sequence;
				}
				// verify roll
				if (Math.abs(predicted_pose[1][2]) > max_roll ) {
					if (debugLevel > -3) {
						System.out.println("getPrescanRange(): roll change exceededs the limit for the scene "+nscene+": "+
								Math.abs(predicted_pose[1][2]*180/Math.PI)+"deg > "+max_roll*180/Math.PI+"deg"); 
					}
					break check_sequence;
				}
				// verify offset(overlap)
				double est_shift = quadCLTs[ref_index].estimateAverageShift(
						new double[2][3], // scenes_xyzatr[ref_index], // double [][] xyzatr0,
						predicted_pose,    // double [][] xyzatr1,
						avg_z,                    // double average_z,
						false,                    // boolean use_rot,
						fmg_rectilinear);                   // boolean fmg_rectilinear)
				if (est_shift < fmg_max_quad) {
					est_shift = quadCLTs[ref_index].estimateAverageShift(
							new double[2][3], // scenes_xyzatr[ref_index], // double [][] xyzatr0,
							predicted_pose,    // double [][] xyzatr1,
							avg_z,                    // double average_z,
							true,                    // boolean use_rot,
							fmg_rectilinear);                   // boolean rectilinear)
				}
				if (est_shift > max_offset ) {
					if (debugLevel > -3) {
						System.out.println("getPrescanRange(): pixel offset exceeds the limit for the scene "+nscene+": "+
								est_shift+"pix > "+max_offset+"pix (too low overlap)"); 
					}
					break check_sequence;
				}
				// check horizontal movement exceeded - just indicate it
				double hor_move = Math.sqrt(predicted_pose[0][0]*predicted_pose[0][0]+predicted_pose[0][1]*predicted_pose[0][1]);
				if (hor_move > max_meters) {
					if (horizontal_OK && (debugLevel > -3)) {
						System.out.println("getPrescanRange(): horizontal movement exceeds the limit for the scene "+nscene+": "+
								hor_move+"m > "+max_meters+"m. Fitting will require to use SFM for smaller scene range to improve disparity accuracy."); 
					}
					horizontal_OK = false;
				}
				continue; // All good, proceed to the next earlier scene
			}
			// some limit is exceeded
			if (center_index < 0) {
				center_index = nscene + 1; // last good one
				compare_index = center_index;
				inv_compare_xyzatr = ErsCorrection.invertXYZATR(ims_xyzatr[compare_index]);
				nscene++; // the outer loop will repeat the current scene once more
				if (debugLevel > -3) {
					System.out.println("getPrescanRange(): Will use scene "+center_index+
							" as a center scene and find the earliest scene that can be referenced to it."); 
				}
			} else {
				earliest_index = nscene + 1;
				if (debugLevel > -3) {
					System.out.println("getPrescanRange(): Will use scene "+earliest_index+
							" ("+quadCLTs[earliest_index].getImageName()+")"+
							" as a the earliest scene in the sequence (referenced to the center scene "+center_index+
							" ("+quadCLTs[center_index].getImageName()+")"); 
				}
				break; // main loop
			}
		}
		if (earliest_index < 0) { // never set
			earliest_index = ealiest_defined;
			int middle_index = (ref_index+earliest_index)/2;
			if (center_index < middle_index) {
				center_index = middle_index;
			}
		}
		int latest_prescan =   ref_index;
		int earliest_prescan = earliest_index;
		if (!horizontal_OK) { // maybe will go away with modified center_index
			horizontal_OK = true;
			double [][] inv_center_xyzatr = ErsCorrection.invertXYZATR(ims_xyzatr[center_index]);
			for (int nscene = center_index+1; (nscene <= ref_index); nscene++) {
				double [][] predicted_pose = ErsCorrection.combineXYZATR(ims_xyzatr[nscene], inv_center_xyzatr);
				double hor_move = Math.sqrt(predicted_pose[0][0]*predicted_pose[0][0]+predicted_pose[0][1]*predicted_pose[0][1]);
				if (hor_move > max_meters) {
					if (horizontal_OK && (debugLevel > -3)) {
						System.out.println("getPrescanRange(): horizontal movement exceeds the limit for the scene "+nscene+": "+
								hor_move+"m > "+max_meters+"m. Fitting will require to use SFM for smaller scene range to improve disparity accuracy."); 
					}
					latest_prescan = nscene - 1;
					horizontal_OK = false;
					break;
				}
			}
			for (int nscene = center_index-1; (nscene >= earliest_index); nscene--) {
				double [][] predicted_pose = ErsCorrection.combineXYZATR(ims_xyzatr[nscene], inv_center_xyzatr);
				double hor_move = Math.sqrt(predicted_pose[0][0]*predicted_pose[0][0]+predicted_pose[0][1]*predicted_pose[0][1]);
				if (hor_move > max_meters) {
					if (horizontal_OK && (debugLevel > -3)) {
						System.out.println("getPrescanRange(): horizontal movement exceeds the limit for the scene "+nscene+": "+
								hor_move+"m > "+max_meters+"m. Fitting will require to use SFM for smaller scene range to improve disparity accuracy."); 
					}
					earliest_prescan = nscene + 1;
					horizontal_OK = false;
					break;
				}
			}
		}
		
		EstimateSceneRange estimateSceneRange = new EstimateSceneRange();
		// verify ranges are sane, return null if not?
		estimateSceneRange.earliest =         earliest_index;
		estimateSceneRange.latest =           ref_index;
		estimateSceneRange.center =           center_index;
		estimateSceneRange.earliest_prescan = earliest_prescan;
		estimateSceneRange.latest_prescan =   latest_prescan;
		estimateSceneRange.needs_prescan =   !horizontal_OK;
		if (debugLevel > -3) {
			System.out.println("getPrescanRange(): "+estimateSceneRange.toString() ); 
		}

		return estimateSceneRange;
	}
	@Override
	public String toString() { // not used in lwir
		return String.format("center=%5d, earliest = %5d, latest = %5d, earliest_prescan = %5d, latest_prescan = %5d, needs_prescan = %b\n",
				center, earliest, latest, earliest_prescan, latest_prescan, needs_prescan );
	}
	
	private static void ensureSfmOnlyReferenceGpu(
			final CLTParameters clt_parameters,
			final QuadCLT[] quadCLTs,
			final int cent_index,
			final boolean[] reliable_ref,
			final double[][] pXpYD_ref,
			final TpTask[][] tp_tasks_ref,
			final int debugLevel) {
		if ((tp_tasks_ref != null) && (tp_tasks_ref.length > 0) && (tp_tasks_ref[0] != null)) {
			return;
		}
		if ((tp_tasks_ref == null) || (tp_tasks_ref.length == 0)) {
			return;
		}
		final boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
		final double[] ref_disparity = quadCLTs[cent_index].getDLS()[use_lma_dsi ? 1 : 0];
		final double[][] pXpYD_ref1 = OpticalFlow.transformToScenePxPyD(
				null,
				ref_disparity,
				Interscene.ZERO3,
				Interscene.ZERO3,
				quadCLTs[cent_index],
				quadCLTs[cent_index]);
		double[][] pXpYD_ref_use = pXpYD_ref1;
		if ((pXpYD_ref != null) && (pXpYD_ref.length >= pXpYD_ref1.length)) {
			System.arraycopy(
					pXpYD_ref1,
					0,
					pXpYD_ref,
					0,
					pXpYD_ref1.length);
			pXpYD_ref_use = pXpYD_ref;
		}
		final TpTask[][] tp_tasks_ref2 = Interscene.setReferenceGPU(
				clt_parameters,
				quadCLTs[cent_index],
				ref_disparity,
				pXpYD_ref_use,
				null,
				reliable_ref,
				clt_parameters.imp.margin,
				0.0,
				0.0,
				null,
				debugLevel);
		System.arraycopy(
				tp_tasks_ref2,
				0,
				tp_tasks_ref,
				0,
				Math.min(
						tp_tasks_ref.length,
						tp_tasks_ref2.length));
	}
	
	
	
	public static boolean scanSfmIMS(
			final CLTParameters           clt_parameters,
			EstimateSceneRange            estimateSceneRange,
			final QuadCLT[]           	  quadCLTs,
			final QuadCLT                 quadCLT_main,
			final ColorProcParameters     colorProcParameters,
			final SetChannels []          set_channels,
			final boolean                 batch_mode,
			final boolean                 updateStatus,
			final int                     debugLevel) {
		return scanSfmIMS(
				clt_parameters,
				estimateSceneRange,
				quadCLTs,
				quadCLT_main,
				colorProcParameters,
				set_channels,
				batch_mode,
				updateStatus,
				debugLevel,
				false);
	}

	public static boolean scanSfmIMS(
			final CLTParameters           clt_parameters,
			EstimateSceneRange            estimateSceneRange,
			final QuadCLT[]           	  quadCLTs,
			final QuadCLT                 quadCLT_main,
			final ColorProcParameters     colorProcParameters,
			final SetChannels []          set_channels,
			final boolean                 batch_mode,
			final boolean                 updateStatus,
			final int                     debugLevel,
			final boolean                 sfm_only) {
		int earliest = estimateSceneRange.earliest; // was earliest_prescan
		int latest =   estimateSceneRange.latest;   // was latest_prescan
		// The current version does not use smaller range (prescan) as it uses SFM
//		double extra_max = 1.2; // increase maximal shift in pixels for not-so-accurate estimation (actual was 1.05)
		RMSEStats rmse_stats = new RMSEStats();
		RMSEStats rmse_stats_metric = clt_parameters.imp.eig_use ? (new RMSEStats()):null;
		final int cent_index = estimateSceneRange.center; //
		int tilesX =    quadCLTs[cent_index].getTileProcessor().getTilesX();
		int tilesY =    quadCLTs[cent_index].getTileProcessor().getTilesY();
		int tile_size = quadCLTs[cent_index].getTileProcessor().getTileSize();
		int tiles = tilesX*tilesY;
//        int tile_size = quadCLTs[cent_index].getTileProcessor().getTileSize();

		boolean  lma_use_Z =         clt_parameters.imp.lma_use_Z;          // true;       // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables  
		boolean  lma_use_R =         clt_parameters.imp.lma_use_R;          // true;       // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
		double   lma_ovlp_ZR =       clt_parameters.imp.lma_ovlp_ZR;        // 0.4;   // minimal overlap to adjust Z and R
		
		boolean  lock_position = false;
		double  min_ref_str =        clt_parameters.imp.min_ref_str;
		boolean ref_need_lma =       clt_parameters.imp.ref_need_lma;
		double  min_ref_frac=        clt_parameters.imp.min_ref_frac;
		boolean sfm_filter =         clt_parameters.imp.sfm_filter;  //true;   // use SfM filtering if available 
		double  sfm_minmax =         clt_parameters.imp.sfm_minmax;  //10.0;   // minimal value of the SfM gain maximum to consider available
		double  sfm_fracmax =        clt_parameters.imp.sfm_fracmax; // 0.75;  // minimal fraction of the SfM maximal gain
		double  sfm_minabs  =        clt_parameters.imp.sfm_minabs; //5.0;  // SFM gain threshold to use min_ref_str_sfm as minimal reliable strength
		double  sfm_fracall =        clt_parameters.imp.sfm_fracall; // 0.3;    // minimal relative area of the SfM-enabled tiles (do not apply filter if less)	
		double  min_ref_str_sfm =    clt_parameters.imp.min_ref_str_sfm; // 0.1;  // reliable strength when sfm gain is available
		boolean use_ims_rotation =   clt_parameters.imp.use_quat_corr;   // use internally (probably deprecated - not)
		boolean inertial_only =      clt_parameters.imp.inertial_only;   // use internally
    	double  fmg_max_quad =       clt_parameters.imp.fmg_max_quad; // estimate offset by 4 points (rooll-aware, 25% from center) if center offset is too small
     	boolean fmg_initial_en =     clt_parameters.imp.fmg_initial_en; // enable IMS-based FPN mitigation for initial orientation
    	double  fmg_distance =       clt_parameters.imp.fmg_distance; // try to find other reference scene not closer than this pixels
    	boolean fmg_rectilinear =    clt_parameters.imp.fmg_rectilinear;// use rectilinear model for scene offset estimation 
		double  min_offset =         clt_parameters.imp.min_offset; // 0.0; // 
//		double  min_offset_estimated=clt_parameters.imp.fpn_min_offset_estimated; // pre-LMA filtering not to have to remove during LMA 
		
		double  max_offset =         Double.NaN;// Do not check * clt_parameters.imp.max_rel_offset * tilesX * tile_size;
		boolean ref_smooth =         clt_parameters.imp.ref_smooth;  //true;   // use SfM filtering if available Not yet available here
		double  min_sfm_meters =     clt_parameters.imp.min_sfm_meters; // 2.5;   // minimal camera offset no use SFM for disparity refinement (~10x camera base)

		boolean  filt_en =           clt_parameters.imp.sfma_filt_en;      // true;   // Filter disparity during pose adjustment with SfM
		boolean  filt_each =         clt_parameters.imp.sfma_filt_each;    // true;   // Filter disparity after each step (far enough), false - only once after all Adjustments are over
		double   filt_meters =       clt_parameters.imp.sfma_filt_meters;  // 5.0;    // Filter only for high offsets
		
		boolean  sfma_all =           clt_parameters.imp.sfma_all_en;       // true;   // calculate SfM for all tiles, including "unreliable" (make them reliable?)
		double   sfma_confidence =    clt_parameters.imp.sfma_confidence;   // 0.8;    // rehabilitate "unreliable" SfM tiles if confidence is above this threshold
		
		double [] lma_rms = new double[clt_parameters.imp.eig_use? 6 : 4]; // [2];
		boolean  save_reliables =     clt_parameters.imp.save_reliables;
		boolean [][] reliables = save_reliables? (new boolean [quadCLTs.length][]) : null;

		// create new dsi for quadCLTs[earliest_scene1]
		OpticalFlow.buildRefDSI( // returned is a different instance than input -FIXED
				clt_parameters,                   // CLTParameters                                 clt_parameters,
				false,                            // boolean  fast,
				true,                             // boolean                                       skip_photo,
				null,                             // ColorProcParameters                           colorProcParameters,
				null,                             // EyesisCorrectionParameters.RGBParameters      rgbParameters,
				batch_mode,                       // boolean                             batch_mode,
				set_channels[cent_index].set_name, // String                              set_name,
				quadCLT_main,                     // QuadCLT                             quadCLT_main, // tiles should be set
				quadCLTs[cent_index],              // QuadCLT                             quadCLT_ref, // tiles should be set
				Interscene.THREADS_MAX,           // final int                                     threadsMax,  // maximal number of threads to launch
				updateStatus,                     // final boolean    updateStatus,
				debugLevel);                      // final int        debugLevel);
		// Saving result to /media/elphel/btrfs-data/lwir16-proc/NC/models/1763232117-1763234145/1763232534_879768/v61/1763232534_879768-DSI_MAIN.tiff
		// Configuration parameters are saved to /media/elphel/btrfs-data/lwir16-proc/NC/models/1763232117-1763234145/1763232534_879768/v61/1763232534_879768-INTERFRAME.corr-xml
		quadCLTs[cent_index] = (QuadCLT) quadCLT_main.spawnQuadCLT( // restores dsi from "DSI-MAIN", uses quat_corr
				set_channels[cent_index].set_name,
				clt_parameters,
				colorProcParameters, //
				Interscene.THREADS_MAX,
				debugLevel);
		quadCLTs[cent_index].setQuadClt(); // just in case ?

		if (debugLevel > -3) {
			System.out.println("scanSfmIMS(): lma_use_Z="+lma_use_Z+", lma_use_R="+lma_use_R+ ", lma_ovlp_ZR="+lma_ovlp_ZR+
					", ref_scene= "+quadCLTs[cent_index].getImageName()+", sfm_only="+sfm_only);
	 	}
		boolean[]    param_select = 
				ErsCorrection.getParamSelect( // ZR - always
						lma_use_Z && !lock_position, // boolean use_Z,
						lma_use_R, // boolean use_R,
						!lock_position, // true,      // boolean use_XY, May change with oorient preference
						lock_position, // false,     // boolean use_AT, May change with oorient preference
						false,     // boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
						false,     // boolean use_ERS_tilt);
						false);    // boolean use_ERS_roll);
		boolean[]    param_select_nozr = 
				ErsCorrection.getParamSelect( // ZR - always
						false,     // boolean use_Z,
						false,     // boolean use_R,
						!lock_position, // true,      // boolean use_XY, May change with oorient preference
						lock_position, // false,     // boolean use_AT, May change with oorient preference
						false,     // boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
						false,     // boolean use_ERS_tilt);
						false);    // boolean use_ERS_roll);
		System.out.print("param_select=[");
		for (int i = 0; i < param_select.length;i++) {
			System.out.print(param_select[i]? "+":"-");
		}
		System.out.println("]");
		
		int []    fail_reason = new int[1];   // null or int[1]: 0 - OK, 2 - LMA, 3 - min, 4 - max
		
		// Probably spawnNoModelQuadCLT() is not needed as it is done in getPrescanRange()
        boolean changed = quadCLTs[cent_index].isPhotometricUpdatedAndReset();
		if (debugLevel > -3) {
			System.out.println("scanSfmIMS(): Before spawnNoModelQuadCLT() changed = "+changed);
		}
		// for now - only the center aprt for prescan
//		for (int scene_index =  estimateSceneRange.earliest_prescan; scene_index <= estimateSceneRange.latest_prescan ; scene_index++) {
		
		// getting rid of the prescan
		for (int scene_index = earliest; scene_index <= latest ; scene_index++) {
			// to include ref scene photometric calibration
			// added 05.21.2024: skip if was spawn already
			if (changed || (quadCLTs[scene_index] == null)) { 
				quadCLTs[scene_index] = quadCLTs[cent_index].spawnNoModelQuadCLT(
						set_channels[scene_index].set_name,
						clt_parameters,
						colorProcParameters, //
						Interscene.THREADS_MAX,
						debugLevel-2);
			}
		} // split cycles to remove output clutter
		
		
		ErsCorrection ers_reference = quadCLTs[cent_index].getErsCorrection();		
//		int debug_scene =            57; // -15; last before
		boolean debug2 =             !batch_mode; // false; // true;
		/* Will calculate coord_motion for every tile reliable_all masked by too-far to avoid folding
		 * but use only reliable_ref tiles for orientation. Possibly, recalculate reliable_ref to include
		 * tiles with good SmF results 
		 */
		boolean [] reliable_ref =    null;
		boolean [] reliable_all =    sfma_all ? (new boolean [tiles]) : null;
		if (reliable_all!= null) {
			Arrays.fill(reliable_all, true);
		}
		boolean use_lma_dsi =        clt_parameters.imp.use_lma_dsi;
        double [] reduced_strength = new double[1];
        // use combo if second pass?
		if (min_ref_str > 0.0) {
			reliable_ref = quadCLTs[cent_index].getReliableTiles( // will be null if does not exist.
					false, // boolean use_combo,
					min_ref_str,          // double min_strength,
					min_ref_frac,         // double min_ref_frac,
					ref_need_lma,         // boolean needs_lma);
					true, // ref_need_lma_combo,   // boolean needs_lma_combo);
					sfm_filter,  // boolean sfm_filter, // use SfM filtering if available                          
					sfm_minmax,  // double  sfm_minmax, // minimal value of the SfM gain maximum to consider available
					sfm_fracmax, // double  sfm_fracmax,// minimal fraction of the SfM maximal gain
					sfm_minabs,      // double    sfm_minabs,
					sfm_fracall,     // double  sfm_fracall,// minimal relative area of the SfM-enabled tiles (do not apply filter if less)
					min_ref_str_sfm, // double    min_ref_str_sfm,
					reduced_strength,  // if not null will return >0 if had to reduce strength (no change if did not reduce)
					debugLevel); // int       debugLevel)
			if (reduced_strength[0] > 0) {
				use_lma_dsi = false; // too few points
			}
			if (debug2) {
				double [] dbg_img = new double [reliable_ref.length];
				for (int i = 0; i < dbg_img.length; i++) {
					dbg_img[i] = reliable_ref[i]?1:0;
				}
				ShowDoubleFloatArrays.showArrays(
						dbg_img,
						quadCLTs[cent_index].getTileProcessor().getTilesX(),
						quadCLTs[cent_index].getTileProcessor().getTilesY(),
						quadCLTs[cent_index].getImageName()+"-reliable_ref");
			}
		} else {
			reliable_ref = new boolean [tiles];
			Arrays.fill(reliable_ref, true);
		}
		if (reliables != null) {
			reliables[cent_index] = reliable_ref;
		}
		double [] quat_corr = use_ims_rotation? quadCLTs[cent_index].getQuatCorr() : null; //
		double [] enu_corr_metric = quadCLTs[cent_index].getENUCorrMetric();
		
		double [][][] scenes_dxyzatr = QuadCLTCPU.getDxyzatrPIMU( // does not depend on correction
				clt_parameters, // CLTParameters clt_parameters,
				quadCLTs,       // QuadCLT[]     quadCLTs,
				quat_corr); // double []  quat_corr, 12/24/1025
		if (debugLevel > -3) {
			System.out.println("scanSfmIMS(): use_ims_rotation="+use_ims_rotation);
			System.out.println("scanSfmIMS(): will "+((quat_corr==null)? "NOT ":"")+"correct orientation offset");
			QuadCLTCPU.showQuatCorr(quat_corr, enu_corr_metric); // enu_corr_metric - linear correction
		}
		// calculate only needed (copy quadCLTs with nulls)?
		double [][][] ims_xyzatr;
		// get IMS poses (now inertial_only== false) , use second branch 
		if (inertial_only) {
			ims_xyzatr = QuadCLT.integratePIMU( // does nor use quat_corr
						clt_parameters, // final CLTParameters clt_parameters,
						quadCLTs,       // final QuadCLT[]     quadCLTs,
						quat_corr,      // double []  quat_corr,
						cent_index,      // final int           ref_index,
						null,           // double [][][]       dxyzatr,
			    		0, // final int           early_index,
			    		(quadCLTs.length -1), // int           last_index,
//			    		(new double[2][3]) //final double [][] pimu_offsets // null - will not subtract from velocities
						debugLevel      // final int           debugLevel
					);
		} else {
			ims_xyzatr = quadCLTs[cent_index].getXyzatrIms(
					clt_parameters, // CLTParameters clt_parameters,
					quadCLTs, // QuadCLT[]  quadCLTs,
					quat_corr, // double []  quat_corr, // only applies to rotations - verify!
					debugLevel) ; // int        debugLevel)
		}
		
		boolean debug_ims_xyzatr = true;
		if (debug_ims_xyzatr) {
			System.out.println(String.format("%6s, %20s, %15s, %15s, %15s, %15s, %15s, %15s", "index","timestamp", "X", "Y", "Z", "Azimuth", "Tilt", "Roll"));
			for (int i =  earliest; i <= latest ; i++) {
				System.out.println(String.format("%6d, %20s, %15.12f, %15.12f, %15.12f, %15.12f, %15.12f, %15.12f", i,quadCLTs[i].getImageName(),
						ims_xyzatr[i][0][0],ims_xyzatr[i][0][1],ims_xyzatr[i][0][2],ims_xyzatr[i][1][0],ims_xyzatr[i][1][1],ims_xyzatr[i][1][2]));
			}
		}
		if (lock_position) { // zero linear offsets
			for (int i = 0; i < quadCLTs.length; i++) for (int j = 0; j < 3; j++){
				ims_xyzatr[i][0][j]=0.0;
			}
		}
		
		double [][][] scenes_xyzatr =      new double [quadCLTs.length][][]; // previous scene relative to the next one
		scenes_xyzatr[cent_index] =         new double[2][3]; // all zeros
		ers_reference.addScene(quadCLTs[cent_index].getImageName(), // add reference scene (itself) too
				scenes_xyzatr[cent_index][0],
				scenes_xyzatr[cent_index][1],
				scenes_dxyzatr[cent_index][0], // cam_dxyzatr_ref[0],  // later may overwrite from overlap		
				scenes_dxyzatr[cent_index][1]); //cam_dxyzatr_ref[1]); // ZERO3);// ers_scene.getErsATR_dt()	
		// Will be used in prepareLMA()
		quadCLTs[cent_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
				QuadCLTCPU.scaleDtToErs(
						clt_parameters,
						scenes_dxyzatr[cent_index])); // cam_dxyzatr_ref));
		
		// Trying new version with motion blur and single-setting of the reference frame			
		boolean mb_en =       clt_parameters.imp.mb_en;
		double  mb_tau =      clt_parameters.imp.mb_tau;      // 0.008; // time constant, sec
		double  mb_max_gain = clt_parameters.imp.mb_max_gain_inter; // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
		TpTask[][]     tp_tasks_ref = new TpTask[2][];
		double [][]    pXpYD_ref = new double [tilesX*tilesY][];
		if (sfm_only) {
			ensureSfmOnlyReferenceGpu(
					clt_parameters,
					quadCLTs,
					cent_index,
					reliable_ref,
					pXpYD_ref,
					tp_tasks_ref,
					debugLevel);
		}
	    	ArrayList<Integer> fpn_list = new ArrayList<Integer>();
		double [] reg_weights = clt_parameters.ilp.ilma_regularization_weights;
		final double       max_rms =     clt_parameters.imp.eig_use? clt_parameters.imp.eig_max_rms: clt_parameters.imp.max_rms;
		double avg_z = quadCLTs[cent_index].getAverageZ(true); // use lma
		boolean        img_debug_LMA = debugLevel>1000; // to set manually for debug
		final double [] sfm_gain2 =      new double [tilesX*tilesY]; // sfm gain squared
		final double [] sfm_confidence = new double [tilesX*tilesY]; // sfm gain squared
		final double [] accum_weights =  new double [tilesX*tilesY]; // sfm gain squared
		int num_sfm = 0;  // do not use if no sfm was performed (for LY with too few scenes and high altitude)
		
		//Iterate from the reference scene back in time, then forward in time
		for (int scan_dir = 1; scan_dir >= -1; scan_dir -=2) {
			double [][] last_corr_xyzatr = {Interscene.ZERO3,Interscene.ZERO3};
			for (int scene_index =  cent_index + scan_dir;
//				(scene_index >= estimateSceneRange.earliest_prescan) && (scene_index <= estimateSceneRange.latest_prescan);
					
					(scene_index >= earliest) && (scene_index <= latest);
					scene_index +=  scan_dir) {
				QuadCLT scene_QuadClt = quadCLTs[scene_index];
				double [][] pose_ims = ims_xyzatr[scene_index];
				
				// Apply correction from the earlier processed scenes (closer to reference)
				double [][] initial_pose = ErsCorrection.combineXYZATR(
						last_corr_xyzatr, // current correction
						pose_ims);        // raw IMS prediction
				scenes_xyzatr[scene_index] = new double [][]{
						initial_pose[0].clone(),
						initial_pose[1].clone()
				};
//				double [] min_max = {min_offset, max_offset, 0.0} ; // {min, max, actual rms)
				double [] min_max = {Double.NaN, max_offset, 0.0} ; // {min, max, actual rms)
				// Refine with LMA
				//boost_zoom_short
				// Will be used in prepareLMA()
				quadCLTs[scene_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
						QuadCLTCPU.scaleDtToErs(
								clt_parameters,
								scenes_dxyzatr[scene_index])); // cam_dxyzatr));		
	// Trying new version with motion blur and single-setting of the reference frame
				// Make sure pixel shift between images is not too small otherwise FPN will dominate the useful features in correlation					
	    		double est_shift = quadCLTs[cent_index].estimateAverageShift(
	    				scenes_xyzatr[cent_index], // double [][] xyzatr0,
	    				scenes_xyzatr[scene_index],    // double [][] xyzatr1,
	    				avg_z,                    // double average_z,
	    				false,                    // boolean use_rot,
	    				fmg_rectilinear);                   // boolean fmg_rectilinear)
	    		if (est_shift < fmg_max_quad) {
	    			est_shift = quadCLTs[cent_index].estimateAverageShift(
	    					scenes_xyzatr[cent_index], // double [][] xyzatr0,
	    					scenes_xyzatr[scene_index],    // double [][] xyzatr1,
	    					avg_z,                    // double average_z,
	    					true,                    // boolean use_rot,
	    					fmg_rectilinear);                   // boolean rectilinear)
	    		}

	      		double [] center_offset_xy =  quadCLTs[cent_index].estimateCenterShiftXY(
						scenes_xyzatr[cent_index],  // double [][] xyzatr0,
						scenes_xyzatr[scene_index],// double [][] xyzatr1,
						avg_z,                     // double average_z,
	    				fmg_rectilinear);                   // boolean fmg_rectilinear)
	      		if (center_offset_xy == null) {
	      			System.out.println("center_offset_xy== null");
	      			
	      		}
	      		
	      		// Add too near tiles to unreliable
	    		boolean [] reliable_scene =  quadCLTs[cent_index].maskByOverlap(
	    				reliable_ref, // boolean [] reliable_ref_tiles,
	    				center_offset_xy); // double []  offset)
	    		boolean [] reliable_all_scene =  quadCLTs[cent_index].maskByOverlap(
	    				reliable_all,      // boolean [] reliable_ref_tiles, (null OK)
	    				center_offset_xy); // double []  offset)
	    		
	    		if (reliables != null) {
	    			reliables[scene_index]=reliable_scene;
	    		}
	    		// calculate linear horizontal shift between camera poses to see if SfM makes sense
				double hor_move = Math.sqrt(initial_pose[0][0]*initial_pose[0][0]+initial_pose[0][1]*initial_pose[0][1]);
				
				// Disable fitting of zoom and roll if overlap is too small (both require significant overlap between the images
	    		boolean no_zr = est_shift > lma_ovlp_ZR * (tilesX * tile_size);
	    		boolean [] param_select_use = no_zr ? param_select_nozr: param_select;
	    		
	       		if (debugLevel > -4) {
	    			System.out.println("nscene="+scene_index+": est offset "+
	    					est_shift+"pix (dx="+center_offset_xy[0]+", dy="+center_offset_xy[1]+"), hor_move="+hor_move+", no_zr="+no_zr);
	    			if (hor_move > min_sfm_meters) {
	    				System.out.println(hor_move +" >" +min_sfm_meters+" - sufficient for SFM disparity refining");
	    			}
	    		}
	       		double [][][]  coord_motion = (hor_move > min_sfm_meters) ? (new double [3][][]) : null; 
	    		boolean adjust_OK = false;
	    		// TODO: add min_offset_initial to other adjustments, make scale configurable
	    		boolean prefiltered = false;
	    		
	    		// If pixel shift between image pairs is too small, skip it to mitigate late, when others scenes are done	
	       		if (est_shift < min_offset) { // min_max[0]) {
	    			fail_reason[0]=Interscene.FAIL_REASON_MIN;
	    			prefiltered = true;
	    		} else if (sfm_only) {
	    			ensureSfmOnlyReferenceGpu(
	    					clt_parameters,
	    					quadCLTs,
	    					cent_index,
	    					reliable_ref,
	    					pXpYD_ref,
	    					tp_tasks_ref,
	    					debugLevel);
	    			coord_motion = Interscene.interCorrPair( // return [tile]{ref_pXpYD, dXdYS}
	    					clt_parameters,      // CLTParameters      clt_parameters,
	    					false,               // boolean            use3D,
	    					false,               // boolean            fpn_disable,
	    					mb_max_gain,         // double             mb_max_gain,
	    					min_max,             // double []          min_max,
	    					fail_reason,         // int []             fail_reason,
	    					quadCLTs[cent_index],// QuadCLT            ref_scene,
	    					null,                // double []          ref_disparity,
	    					quadCLTs[cent_index],// QuadCLT            first_scene,
	    					pXpYD_ref,           // double [][]        pXpYD_ref,
	    					tp_tasks_ref[0],     // TpTask[]           tp_tasks_ref,
	    					quadCLTs[scene_index], // QuadCLT          scene,
	    					initial_pose[0],     // double []          scene_xyz,
	    					initial_pose[1],     // double []          scene_atr,
	    					reliable_scene,      // boolean []         selection,
	    					clt_parameters.imp.margin, // int            margin,
	    					clt_parameters.imp.sensor_mask_inter, // int    sensor_mask_inter,
	    					null,                // float [][][]       accum_2d_corr,
	    					null,                // float [][]         dbg_corr_fpn,
	    					false,               // boolean            near_important,
	    					false,               // boolean            all_fpn,
	    					true,                // boolean            initial_adjust,
	    					null,                // double [][]        mb_vectors,
	    					-1,                  // int                niter,
	    					img_debug_LMA,       // boolean            img_debug,
	    					clt_parameters.imp.debug_level, // int          imp_debug_level
	    					debugLevel);         // int                debug_level
	    			adjust_OK =
	    					(coord_motion != null) &&
	    					(coord_motion.length >= 2) &&
	    					(coord_motion[0] != null) &&
	    					(coord_motion[1] != null);
	    			if (adjust_OK) {
	    				scenes_xyzatr[scene_index] = new double [][]{
	    						initial_pose[0].clone(),
	    						initial_pose[1].clone()
	    				};
	    				lma_rms[0] = (min_max.length > 2) ? min_max[2] : 0.0;
	    				if (lma_rms.length > 2) {
	    					lma_rms[2] = 0.0;
	    				}
	    				if (lma_rms.length > 3) {
	    					lma_rms[3] = 0.0;
	    				}
	    				if (lma_rms.length > 4) {
	    					lma_rms[4] = 0.0;
	    				}
	    			}
	    		} else {
	    			//		boolean        img_debug_LMA = debugLevel>1000; // to set manually for debug 
//	    			pXpYD_ref = new double [tilesX*tilesY][]; // reset it not to use previous value not needed, controlled by tp_tasks_ref[0]
//min_sfm_meters
	    			scenes_xyzatr[scene_index] = Interscene.adjustDiffPairsLMAInterscene( // compare two scenes, first may be reference, use motion blur
	    					clt_parameters,      // CLTParameters  clt_parameters,
	    					use_lma_dsi,         // clt_parameters.imp.use_lma_dsi,
	    					false,               //	boolean        fpn_disable,   // disable fpn filter if images are known to be too close
	    					true,                // boolean        disable_ers,
	    					min_max,             // double []      min_max,       // null or pair of minimal and maximal offsets
	    					fail_reason,         // int []         fail_reason,   // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
	    					quadCLTs,            // QuadCLT []     quadCLTs,
	    					cent_index,          // int            ref_index,
	    					tp_tasks_ref,        // TpTask[][]     tp_tasks_ref,  // Should be TpTask[2][*]. If tp_tasks_ref[0] == null will calculate
	    					// at set first scene to the GPU
	    					pXpYD_ref,           // double [][]    pXpYD_ref,     // should be set or at least double [num_tiles][] if tp_tasks_ref[0] == null 			
	    					cent_index,          // int            nscene0,       // may be == ref_index
	    					scene_index,         // int            nscene1,       // compares to nscene0
	    					null,                // double []      ref_disparity, // null or alternative reference disparity
	    					reliable_scene,      // reliable_ref,        // boolean []     reliable_ref, // null or bitmask of reliable reference tiles
	    					reliable_all_scene,  // boolean []     reliable_all, // if not null, use to calculate correlations, mask by ref_disparity for pose adjustments
	    					ref_smooth,          // boolean        smooth_disparity, // smooth disparity (according to clt_parameters)+update reliable_ref if true
	    					scenes_xyzatr[cent_index], // double [][]    scene0_xyzatr,,
	    					initial_pose,        // double [][]    scene1_xyzatr,
	    					Double.NaN,          // double         average_z,     
	    					initial_pose,        // double []      scene1_xyzatr_pull, // if both are not null, specify target values to pull to 
	    					param_select_use,    // clt_parameters.ilp.ilma_lma_select, // boolean[]      param_select,
	    					reg_weights,         // double []      param_regweights,
	    					lma_rms,             // double []      rms_out, // null or double [2]
	    					max_rms,             // double         max_rms,
	    					mb_en,               // boolean        mb_en,
	    					mb_tau,              // double         mb_tau,      // 0.008; // time constant, sec
	    					mb_max_gain,         // double         mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
	    					coord_motion,        // double [][][]  coord_motion, // if not null should be double[3][][], will return correlation result
	    					img_debug_LMA,       // boolean        img_debug, // generate debug images for each LMA iteration (dangerous, may be many images)
	    					clt_parameters.imp.debug_level); // int            debugLevel);
	    			adjust_OK = scenes_xyzatr[scene_index] != null;
	    		}
		      		// skipping roll and zoom check - they are done earlier.
	    		if (!adjust_OK) {
	    			System.out.println("scanSfmIMS(): "+(sfm_only ? "interCorrPair" : "LMA")+
	    					" failed at nscene = "+scene_index+". Reason = "+fail_reason[0]+
	    					" ("+Interscene.getFailReason(fail_reason[0])+")"+
	    					(prefiltered? (". It was prefiltered as est_shift="+est_shift+" < "+min_offset):""));
	    			if (fail_reason[0]==Interscene.FAIL_REASON_MIN) {
	    				fpn_list.add(scene_index);
	    				scenes_xyzatr[scene_index] = initial_pose;
	    				// should not be modified for initial adjust, but just in case
	    				quadCLTs[scene_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
	    						QuadCLTCPU.scaleDtToErs(
	    								clt_parameters,
	    								scenes_dxyzatr[scene_index]));		
	    			} else {
	    				// all other reasons lead to failure, but here just use initial estimation
	    				scenes_xyzatr[scene_index] = initial_pose;
	    				// should not be modified for initial adjust, but just in case
	    				quadCLTs[scene_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
	    						QuadCLTCPU.scaleDtToErs(
	    								clt_parameters,
	    								scenes_dxyzatr[scene_index]));
	    				
	    				
	    			}	
	    		} // if (!adjust_OK) {
	    		
				// update	last_corr_xyzatr, // current correction
				double [][] corr_diff = ErsCorrection.combineXYZATR(
						scenes_xyzatr[scene_index],                // new
						ErsCorrection.invertXYZATR(initial_pose)); //old
				last_corr_xyzatr = ErsCorrection.combineXYZATR (
						last_corr_xyzatr,
						corr_diff);
				// not used here with IMS: after_spiral == false
				
				double [][] adjusted_xyzatr_dt = QuadCLTCPU.scaleDtFromErs(
						clt_parameters,
						quadCLTs[scene_index].getErsCorrection().getErsXYZATR_dt(
								));
				
				// Add adjustment results to the list that later will be saved to the .corr-xml file
				ers_reference.addScene(scene_QuadClt.getImageName(),
						scenes_xyzatr[scene_index][0],
						scenes_xyzatr[scene_index][1],
						adjusted_xyzatr_dt[0], // ZERO3, // ers_scene.getErsXYZ_dt(),		
						adjusted_xyzatr_dt[1] // ZERO3 // ers_scene.getErsATR_dt()		
						);
				rmse_stats.add(lma_rms[0]);
				if ((rmse_stats_metric != null) && (lma_rms.length >=4)) {
					rmse_stats_metric.add(lma_rms[4]);
				}
				if (debugLevel > -3) {
					System.out.println("Pass multi scene "+scene_index+" (of "+ quadCLTs.length+") "+
							quadCLTs[cent_index].getImageName() + "/" + scene_QuadClt.getImageName()+
			                " Done. Weight = "+lma_rms[2]+", number="+lma_rms[3]);
			        System.out.print("RMS="+lma_rms[0]+
			                ", maximal so far was "+rmse_stats.getMax()+", average was "+rmse_stats.getAverage());
			        if ((rmse_stats_metric != null) && (lma_rms.length >=6)) {
			            System.out.print(". Pixel RMS="+lma_rms[4]+
			                    "pix, maximal so far was "+rmse_stats_metric.getMax()+
			                    "pix, average was "+rmse_stats_metric.getAverage()+"pix.");
			        }
			        System.out.println();
				}
				if (adjust_OK && (coord_motion != null)) { // adjust SFM // compare pXpYD_ref with coord_motion[0] - is it the same?
					/* Now coord_motionm may have all tiles, not only "reliable" (too far are still excluded in
					 * reliable_all_scene)
					 * TODO: use reliable_scene to use by neighbors, update all with with non-null (and strong enough?) correlations.
					 * 
					 * Rehabilitate all strong enough?
					 *  
					 */
					boolean sfm_OK = adjustDisparitySFM( // modifies one slice of the quadCLTs[cent_index].dsi
							clt_parameters, // final CLTParameters           clt_parameters,
							quadCLTs,       // final QuadCLT[]           	 quadCLTs,
							cent_index,     // final int                     ref_index,
							scene_index,    // final int                     scene_index,
							reliable_scene, // final boolean []              reliable_scene, // updated with new good 
							reliable_all_scene, // boolean [] reliable_all_scene
							coord_motion,   // final double [][][]           coord_motion,
							sfm_gain2,		// final double []               sfm_gain2, // squared
							sfm_confidence, // final double []               sfm_confidence,
							accum_weights,   // final double []              accum_weights,
							batch_mode, 	// final boolean                 batch_mode,
							debugLevel);    // final int                     debugLevel)
					if (debugLevel > -3) {
						System.out.println("Adjusted disparity SFM, result "+sfm_OK+", hor_move = "+hor_move);
					}
					if (reliable_all_scene != null) {
						for (int ntile = 0; ntile < reliable_scene.length; ntile++) {
							reliable_ref[ntile] |= reliable_scene[ntile];
						}
					}
					
					if (filt_en && filt_each && (hor_move >= filt_meters)) {
						boolean sfm_filter_OK = FilterDisparitySFM(
								clt_parameters, // final CLTParameters           clt_parameters,
								false,          // final boolean                 final_filter, // true after all pose adjustments are finished, fals at intermediate steps
								quadCLTs,       // final QuadCLT[]           	  quadCLTs,
								cent_index,     //final int                     ref_index,
								reliable_scene, // final boolean []              reliable,
								scene_index,    // final int                     scene_index, // for debug, null _ final
								batch_mode, 	// final boolean                 batch_mode,
								debugLevel);    // final int                     debugLevel)
						if (debugLevel > -3) {
							System.out.println("Filtered disparity SFM, result "+sfm_filter_OK);
						}
					}
					tp_tasks_ref[0] = null; // force re-calculate from disparity inside next  Interscene.adjustDiffPairsLMAInterscene( )
					num_sfm++; // number of performed SFMs
				}
			} //for (int scene_index =  cent_index + scan_dir;...			
		} // for (int scan_dir = 1; scan_dir >= -1; scan_dir -=2) {
		if (debugLevel > -4) {
			System.out.println("num_fpn_mitigate= "+fpn_list.size());
		}
		// Mitigate FPN - orient the scenes that were too close to the reference by matching them to other, not-so-close scenes
		// with already known poses
		if (!sfm_only && fmg_initial_en && !fpn_list.isEmpty()) {
			// here max_offset is not critical, min_offset can be 0 too
//			double [] min_max = {min_offset, max_offset, 0.0} ; // {min, max, actual rms)
			double [] min_max = {Double.NaN, max_offset, 0.0} ; // {min, max, actual rms)
			if (fmg_distance < (min_max[0] + 2)) {
				fmg_distance = min_max[0] + 2;
			}
			int [][] fpn_pairs = Interscene.getFPNPairs(
					fpn_list,                     // ArrayList<Integer> fpn_list,
					fmg_distance,                 // double             fpn_mitigate_dist,
					fmg_rectilinear,              // boolean            rectilinear,
					quadCLTs,                     // QuadCLT []         quadCLTs,
					scenes_xyzatr,                // double [][][]      scenes_xyzatr,
					avg_z,                        // double             avg_z,
					latest, // estimateSceneRange.latest_prescan,    // int                ref_index, // >= earliest_scene 
					earliest); // estimateSceneRange.earliest_prescan); // int                earliest_scene)
			// mitigating problem, that in the process of adjusting offset can fall below
			double [] min_max_fpn = {0,min_max[1]}; 
			for (int ipair = 0; ipair < fpn_pairs.length; ipair++) if (fpn_pairs[ipair][1] >= 0) {
				if (debugLevel > -4) {
					System.out.println("Mitigating FPN for scene "+fpn_pairs[ipair][0]+
							" being too close to reference "+cent_index+
							", using scene "+fpn_pairs[ipair][1]+" as a reference");
				}
				
	      		double [] center_offset_xy0 =  quadCLTs[cent_index].estimateCenterShiftXY(
						scenes_xyzatr[cent_index], // double [][] xyzatr0,
						scenes_xyzatr[fpn_pairs[ipair][0]],    // double [][] xyzatr1,
						avg_z,                    // double average_z,
	    				fmg_rectilinear);                   // boolean fmg_rectilinear)
	    		boolean [] reliable_scene =  quadCLTs[cent_index].maskByOverlap(
	    				reliable_ref, // boolean [] reliable_ref_tiles,
	    				center_offset_xy0); // double []  offset)
	      		double [] center_offset_xy1 =  quadCLTs[cent_index].estimateCenterShiftXY(
						scenes_xyzatr[cent_index],           // double [][] xyzatr0,
						scenes_xyzatr[fpn_pairs[ipair][1]], // double [][] xyzatr1,
						avg_z,                              // double average_z,
	    				fmg_rectilinear);                   // boolean fmg_rectilinear)
	    		reliable_scene =  quadCLTs[cent_index].maskByOverlap(
	    				reliable_scene, // boolean [] reliable_ref_tiles,
	    				center_offset_xy1); // double []  offset)
				TpTask[][]     tp_tasks_rel_ref = new TpTask[2][];
				// no need to process SFM - it is too close to the reference
    			double [][] rel_xyzatr = Interscene.adjustDiffPairsLMAInterscene( // compare two scenes, first may be reference, use motion blur
    					clt_parameters,      // CLTParameters  clt_parameters,
    					use_lma_dsi,         // clt_parameters.imp.use_lma_dsi,
    					false,               //	boolean        fpn_disable,   // disable fpn filter if images are known to be too close
    					true,                // boolean        disable_ers,
    					min_max_fpn,         // min_max,// double []   min_max,       // null or pair of minimal and maximal offsets
    					fail_reason,         // int []         fail_reason,   // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
    					quadCLTs,            // QuadCLT []     quadCLTs,
    					cent_index,          // int            ref_index,
    					tp_tasks_rel_ref,    // TpTask[][]     tp_tasks_ref,  // Should be TpTask[2][*]. If tp_tasks_ref[0] == null will calculate
    					// at set first scene to the GPU
    					pXpYD_ref,           // double [][]    pXpYD_ref,     // should be set or at least double [num_tiles][] if tp_tasks_ref[0] == null 			
    					fpn_pairs[ipair][1], // ref_index,           // int            nscene0,       // may be == ref_index
    					fpn_pairs[ipair][0], // int            nscene1,       // compares to nscene0
    					null,                // double []      ref_disparity, // null or alternative reference disparity
    					reliable_scene,      // reliable_ref,        //boolean []      reliable_ref, // null or bitmask of reliable reference tiles
    					null,                // boolean []     reliable_all, // if not null, use to calculate correlations, mask by ref_disparity for pose adjustments
    					ref_smooth,          // boolean        smooth_disparity, // smooth disparity (according to clt_parameters)+update reliable_ref if true
    					scenes_xyzatr[fpn_pairs[ipair][1]], // double [][]    scene0_xyzatr,,
    					scenes_xyzatr[fpn_pairs[ipair][0]], // initial_pose,        // double [][]    scene1_xyzatr,
						Double.NaN,          //  double         average_z,     
    					scenes_xyzatr[fpn_pairs[ipair][0]], // initial_pose,        // double []      scene1_xyzatr_pull, // if both are not null, specify target values to pull to 
    					param_select, // clt_parameters.ilp.ilma_lma_select, // boolean[]      param_select,
    					reg_weights,         // double []      param_regweights,
    					lma_rms,             // double []      rms_out, // null or double [2]
    					max_rms,             // double         max_rms,
    					mb_en,               // boolean        mb_en,
    					mb_tau,              // double         mb_tau,      // 0.008; // time constant, sec
    					mb_max_gain,         // double         mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
    					null,                // double [][][]  coord_motion, // if not null should be double[3][][], will return correlation result
    					img_debug_LMA,       // boolean        img_debug, // generate debug images for each LMA iteration (dangerous, may be many images)
    					clt_parameters.imp.debug_level); // int            debugLevel);
    			boolean adjust_OK = rel_xyzatr != null;
				if (!adjust_OK) {
					// should not be modified for initial adjust, but just in case
					quadCLTs[fpn_pairs[ipair][0]].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
							QuadCLTCPU.scaleDtToErs(
									clt_parameters,
									scenes_dxyzatr[fpn_pairs[ipair][0]]));		
					System.out.println("LMA failed at scene pair = "+fpn_pairs[ipair][0]+
							" referenced from scene "+fpn_pairs[ipair][1]+". Reason = "+fail_reason[0]+
							" ("+Interscene.getFailReason(fail_reason[0])+")");
				} else {
					scenes_xyzatr[fpn_pairs[ipair][0]] = rel_xyzatr; // save directly, no need to combine
					double [][] adjusted_xyzatr_dt = QuadCLTCPU.scaleDtFromErs(
							clt_parameters,
							quadCLTs[fpn_pairs[ipair][0]].getErsCorrection().getErsXYZATR_dt());
					ers_reference.addScene(quadCLTs[fpn_pairs[ipair][0]].getImageName(),
							scenes_xyzatr[fpn_pairs[ipair][0]][0],
							scenes_xyzatr[fpn_pairs[ipair][0]][1],
							adjusted_xyzatr_dt[0], // ZERO3, // ers_scene.getErsXYZ_dt(),		
							adjusted_xyzatr_dt[1]); // ZERO3 // ers_scene.getErsATR_dt()		
				    rmse_stats.add(lma_rms[0]);
				    if ((rmse_stats_metric != null) && (lma_rms.length >=4)) {
				        rmse_stats_metric.add(lma_rms[4]);
				    }
					if (debugLevel > -3) {
						System.out.println("Pass multi scene "+fpn_pairs[ipair][0]+" (of "+ quadCLTs.length+") "+
								quadCLTs[fpn_pairs[ipair][1]].getImageName() + "/" + quadCLTs[fpn_pairs[ipair][0]].getImageName()+
								" Done. Weight = "+lma_rms[2]+", number="+lma_rms[3]);
						System.out.print("RMS="+lma_rms[0]+
								", maximal so far was "+rmse_stats.getMax()+", average was "+rmse_stats.getAverage());
						if ((rmse_stats_metric != null) && (lma_rms.length >=6)) {
							System.out.print(". Pixel RMS="+lma_rms[4]+
									"pix, maximal so far was "+rmse_stats_metric.getMax()+
									"pix, average was "+rmse_stats_metric.getAverage()+"pix.");
						}
				        System.out.println();
					}
				}
			}
		} // if (fmg_initial_en && !fpn_list.isEmpty())
		
		if (reliables != null) {
			quadCLTs[cent_index].saveReliables(
					quadCLTs, // QuadCLTCPU [] quadCLTs,
					reliables, // boolean  [][] reliables);
					"-prescan-initial"); // String suffix)
		}
		// perform final filtering of the SFM-found disparities
		if (debugLevel > -3) {
			System.out.println("Number of scenes with SfM =  "+num_sfm);
		}
		if (num_sfm > 0) {
			if (filt_en) { // check weights - min non-zero number
				boolean sfm_filter_OK = FilterDisparitySFM(
						clt_parameters, // final CLTParameters           clt_parameters,
						true,           // final boolean                 final_filter, // true after all pose adjustments are finished, fals at intermediate steps
						quadCLTs,       // final QuadCLT[]           	  quadCLTs,
						cent_index,     //final int                     ref_index,
						reliable_ref,   // final boolean []              reliable,
						-1,             // final int                     scene_index, // for debug, null _ final
						batch_mode, 	// final boolean                 batch_mode,
						debugLevel);    // final int                     debugLevel)
				if (debugLevel > -3) {
					System.out.println("Final filtered disparity SFM, result "+sfm_filter_OK);
				}
			}
			// Set both DSI_DISPARITY_AUX/MAIN and DSI_DISPARITY_AUX/MAIN_LMA to the one adjusted
			final double [] disparity_sfm = quadCLTs[cent_index].getDLS()[clt_parameters.imp.use_lma_dsi?1:0];  
			quadCLTs[cent_index].setDisparityDSI   (disparity_sfm.clone());
			quadCLTs[cent_index].setDisparityLmaDSI(disparity_sfm.clone());
			quadCLTs[cent_index].setStrengthDSI    (sfm_confidence.clone());
			// TODO: Maintain (similar to sfm_gain2) and set strength??
			// Do not yet set combo dsi - copy after disparity correction from IMS
			quadCLTs[cent_index].setSFMGain2(sfm_gain2);
		} // if (num_sfm > 0) {
  		
		// Now always, then - conditional
		boolean adjust_disparity_ims = clt_parameters.imp.air_mode_en;
		boolean apply_disparity_ims =  clt_parameters.imp.air_disp_corr;
		// maybe skip here if it is too short? Do after full?
		if (adjust_disparity_ims) {
	    	double scale_img = OpticalFlow.getImgImsScale( // correctInfinityFromIMS(
	    			quadCLTs, // QuadCLT []       quadCLTs,
	    			quadCLTs[cent_index], // QuadCLT          master_CLT)
	    			earliest, // estimateSceneRange.earliest_prescan);     // int              earliest_scene)
	    			latest); // int              latest_scene) { // -1 will use quadCLTs.length -1;
	    	if (debugLevel > -3) {
	    		System.out.println("ESR: scale_img= "+scale_img+ "in reference scene "+quadCLTs[cent_index].getImageName());
	    	}
			
			double inf_disp = OpticalFlow.getImsDisparityCorrection(
					scale_img,                      // double           scale_img,
					quadCLTs[cent_index],           // QuadCLT          master_CLT,
					clt_parameters.imp.use_lma_dsi, // boolean          use_lma_dsi,
					debugLevel);         // final int        debugLevel) {
	    	if (debugLevel > -3) {
	    		System.out.println("ESR: Disparity at infinity ="+inf_disp+" in reference scene "+quadCLTs[cent_index].getImageName()+", scale_img="+scale_img);
	    	}
	    	
	    	if (apply_disparity_ims) { // make sure - which DSI it adjusts when using SFM
	    		// Update DSI_MAIN with disparity at infinity. Store it somewhere in quadCLTs[ref_index]
	    		quadCLTs[cent_index].offsetDSI(  // applies to all disparity slices INCREMENTS twice?
	    				inf_disp); // double inf_disp)
	    		quadCLTs[cent_index].setDispInfinityRef( // may use incDispInfinityRef() - was 0 before
	    				inf_disp); // double disp)
	    		// correct xyz
	    		OpticalFlow.scaleImgXYZ(
	    				1.0/scale_img, //  double           scale_xyz,
	    				quadCLTs, //, // QuadCLT []       quadCLTs,
	    				quadCLTs[cent_index]); //QuadCLT          master_CLT)
	    		quadCLTs[cent_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
	    				null, // String path,             // full name with extension or w/o path to use x3d directory
	    				//							 	null, // Properties properties,   // if null - will only save extrinsics)
	    				debugLevel);

	    		if (debugLevel > -3) {
	    			System.out.println("Updating DSI-MAIN with updated disparity at infinity for reference scene "+quadCLTs[cent_index].getImageName());
	    		}
	    		quadCLTs[cent_index].saveDSIAll (
	    				"-DSI_MAIN", // String suffix, // "-DSI_MAIN"
	    				quadCLTs[cent_index].dsi);
	    	} else {
		    	if (debugLevel > -3) {
		    		System.out.println("Skipping application of disparity adjustment in reference scene "+quadCLTs[cent_index].getImageName()+", scale_img="+scale_img);
		    	}
	    	}
		}
		double [][] combo_dsn = quadCLTs[cent_index].comboFromMainFgBg(); // set FG/BG layers to avoid null pointers
		QuadCLTCPU.cloneInner(combo_dsn); // separate data array from its source
		if (num_sfm > 0) {
		quadCLTs[cent_index].saveDoubleArrayInModelDirectory(
				QuadCLTCPU.DSI_SUFFIXES[0],   // String      suffix, always with LMA
				OpticalFlow.COMBO_DSN_TITLES, // combo_dsn_titles_full, // null,          // String []   labels, // or null
				combo_dsn,                    // dbg_data,         // double [][] data,
				tilesX,                       // int         width,
				tilesY);                      // int         height)
		} else {
	    	if (debugLevel > -3) {
	    		System.out.println("WARNING: Will not save INTER-INTRA as no SfM measurements were performed (can be high altitude during LY");
	    	}
		}
		quadCLTs[cent_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)  // null pointer
				null, // String path,             // full name with extension or w/o path to use x3d directory
				debugLevel+1);
		// Add to log
    	StringBuffer sb = new StringBuffer();
    	sb.append(new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(Calendar.getInstance().getTime())+"\n");
		sb.append("Finished scanSfmIMS():\n");
    	sb.append("getNumOrient()=   "+quadCLTs[cent_index].getNumOrient()+" of "+clt_parameters.imp.min_num_orient+"\n");
    	sb.append("getNumAccum()=    "+quadCLTs[cent_index].getNumAccum()+ " of "+clt_parameters.imp.min_num_interscene+"\n");
    	sb.append("earliest=         "+earliest+"\n"); // estimateSceneRange.earliest
    	sb.append("earliest_prescan= "+estimateSceneRange.earliest_prescan+" (not used)\n");
    	sb.append("cent_index=       "+cent_index+"\n");
    	sb.append("latest_prescan=   "+estimateSceneRange.latest_prescan+" (not used)\n");
    	sb.append("latest=           "+latest+"\n"); // estimateSceneRange.latest
    	sb.append("Maximal RMSE=     "+rmse_stats.getMax()+"\n");
    	sb.append("Average RMSE=     "+rmse_stats.getAverage()+"\n");
    	if (rmse_stats_metric != null) {
    		sb.append("Maximal RMSE=     "+rmse_stats_metric.getMax()+"pix\n");
	    	sb.append("Average RMSE=     "+rmse_stats_metric.getAverage()+"pix\n");
    	}
    	sb.append("------------------------\n\n");
		quadCLTs[cent_index].appendStringInModelDirectory(sb.toString(), QuadCLT.ORIENTATION_LOGS_SUFFIX); // String  suffix)
		return true;
	}
	
	public static boolean adjustDisparitySFM(
			final CLTParameters           clt_parameters,
			final QuadCLT[]           	  quadCLTs,
			final int                     ref_index, // probably (TODO: verify) non-nulls correspond to reliable_all_scene
			final int                     scene_index,
			final boolean []              reliable,     // used for pose adjustment. Updated with new good!
			final boolean []              reliable_all, // evaluate and possibly "rehabilitate" (if not yet reliable) tiles			
			final double [][][]           coord_motion,
			final double []               sfm_gain2, // squared. Updated here
			final double []               sfm_confidence,
			final double []               accum_weights,
			final boolean                 batch_mode,
			final int                     debugLevel) {
		boolean debug_src = debugLevel > 1000;
		boolean test_nan = debugLevel < 1000;
		double [][] ref_pXpYD = coord_motion[0]; // .clone();
		double [][] mvect =     coord_motion[1]; // dx, dy,conf,?,?
		QuadCLT [] scenes = {quadCLTs[ref_index], quadCLTs[scene_index]};
		ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection();	
		double [][][] scenes_xyzatr = {new double[2][3], ers_reference.getSceneXYZATR(quadCLTs[scene_index].getImageName())};
		double     range_disparity_offset = 0; // clt_parameters.imp.range_disparity_offset; 
		final boolean use_lma_dsi =        clt_parameters.imp.use_lma_dsi;
		//    		boolean  sfma_all =           clt_parameters.imp.sfma_all_en;       // true;   // calculate SfM for all tiles, including "unreliable" (make them reliable?)
		double   sfma_confidence =    clt_parameters.imp.sfma_confidence;   // 0.8;    // rehabilitate "unreliable" SfM tiles if confidence is above this threshold
		final double flatness_ref = 0.05; // pix
		//			final boolean  filt_en =        clt_parameters.imp.sfma_filt_en;      // true;   // Filter disparity during pose adjustment with SfM
		//			final boolean  filt_each =      clt_parameters.imp.sfma_filt_each;    // true;   // Filter disparity after each step (far enough), false - only once after all Adjustments are over
		//			final double   filt_meters =    clt_parameters.imp.sfma_filt_meters;  // 5.0;    // Filter only for high offsets			
		if (test_nan) {
			for (int i = 0; i < sfm_gain2.length; i++) {
				if (Double.isNaN(sfm_confidence[i])) {
					System.out.println("adjustDisparitySFM() start: sfm_confidence["+i+"] = null");
				}
				if (Double.isNaN(sfm_gain2[i])) {
					System.out.println("adjustDisparitySFM() start: sfm_gain2["+i+"] = null");
				}
				if (Double.isNaN(accum_weights[i])) {
					System.out.println("adjustDisparitySFM()start: accum_weights["+i+"] = null");
				}
			}
		}
		double [][] dpXYddisp = StructureFromMotion.getSfmDpxDpyDdisp(
				scenes_xyzatr, // final double [][][] scenes_xyzatr,
				scenes[0], // final QuadCLT       scene0,
				scenes[1], // final QuadCLT       scene1,
				quadCLTs[ref_index],    // final QuadCLT       ref_QuadClt,
				ref_pXpYD,              // final double [][]   ref_pXpYD, // centers
				range_disparity_offset, // final double        range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ;
				batch_mode,             // final boolean       batch_mode,
				debugLevel);            // final int           debug_level
		int tilesX = quadCLTs[ref_index].getTilesX();
		int tilesY = quadCLTs[ref_index].getTilesY();
		int tiles = tilesX*tilesY;

		//       		double [][] dsi = quadCLTs[ref_index].getDLS();
		final double [] ref_disparity =  quadCLTs[ref_index].getDLS()[use_lma_dsi?1:0];
		// Fill NaN here - it will be fast when filled earlier
		if (reliable_all != null) { // should be always
			/* That means that will process all (considering overlap) tiles, so need to fill NaN disparity
			 * with some initial approximation. Likely gaps correspond to high (near) objects - hope SfM
			 * can still work. Maybe start with lower horizontal offsets for such potentially high objects?
			 * Should be before smoothDisparity()
			 */
			boolean [] prohibit = reliable_all.clone();
			for (int i = 0; i < prohibit.length; i++) {
				prohibit[i] = !prohibit[i];
			}
			double [] ref_disparity1 = TileProcessor.fillNaNs(
					ref_disparity, // final double [] data,
					prohibit,      // final boolean [] prohibit,
					tilesX,        // int       width,
					// CAREFUL ! Remaining NaN is grown by unsharp mask filter ************* !
					2* tilesX, // 100, // 2*width, // 16,           // final int grow,
					0.7,          // double    diagonal_weight, // relative to ortho
					100,          // int       num_passes,
					0.03);         // final double     max_rchange, //  = 0.01 - does not need to be accurate
			System.arraycopy(ref_disparity1, 0, ref_disparity, 0, ref_disparity.length);
		}





		double [] ddisp = new double [tiles];
		double [] sfm_gain2_old = debug_src? sfm_gain2.clone() : null;
		double [] sfm_gain2_new = debug_src? (new double [tiles]) : null;
		//	       	Arrays.fill(ddisp, Double.NaN);
		for (int ntile = 0; ntile < tiles; ntile++) {
			double [] dxy = dpXYddisp[ntile];
			if ((mvect[ntile] != null) && (dxy != null) && reliable_all[ntile]) {
				double sfm_g2 = dxy[0]*dxy[0]+dxy[1]*dxy[1];
				double conf = mvect[ntile][2];
				if ((sfm_g2 <=0) || Double.isNaN(sfm_g2) || (conf <= 0) || Double.isNaN(conf) ) {
					System.out.println("adjustDisparitySFM(): **** BUG **** sfm_g2="+sfm_g2+" conf="+conf+" for tile="+ntile);
					continue;
				}
				if (sfm_gain2_new != null) {
					sfm_gain2_new[ntile] = sfm_g2; // for debug images
				}
				//	       			if (sfm_g2 >= sfm_gain2[ntile]) { // only update if the current pair has a larger stereo-base that used
				/* 
				 * Will use even lower sfm_gain so second half contributions will count too (just according to their weights)
				 */
				double dd = -(mvect[ntile][0]*dxy[0] + mvect[ntile][1]*dxy[1]) / sfm_g2; 
				if (Double.isNaN(dd)) {
					System.out.println("adjustDisparitySFM(): dd="+dd);
				}
				ddisp[ntile] =          dd; // used for debug only, not scaled
				double w = conf *  sfm_g2;
				double w_old = accum_weights[ntile];
				double w_new = w_old+w;
				accum_weights[ntile] =  w_new;
				ref_disparity[ntile] -= dd *  w/w_new;
				sfm_confidence[ntile] = (sfm_confidence[ntile] * w_old + w * conf) / w_new;
				sfm_gain2[ntile] = (sfm_gain2[ntile] * w_old + w * sfm_g2)  / w_new;
				//	       				sfm_gain2[ntile] =    sfm_g2; // maximal used
				//	       			}
			}
		}
		boolean [] reliable_new = (reliable_all != null) ? (new boolean  [reliable.length]) : null;
		if (reliable_new != null) { // should be always
			for (int ntile = 0; ntile < reliable_new.length; ntile++) {
				reliable_new[ntile] =
						reliable_all[ntile] && 
						!reliable[ntile] && 
						(mvect[ntile] != null) &&
						(mvect[ntile][2] > sfma_confidence);
			}

		}	       	

		if (debug_src) {
			double [] disparity = new double [tiles];
			for (int ntile = 0; ntile < disparity.length; ntile++) {
				disparity[ntile] = (ref_pXpYD[ntile] != null) ? ref_pXpYD[ntile][2] : Double.NaN;
			}
			double [] curvature2 = getSquaredCurvature(
					disparity, // final double [] data,
					tilesX);   // final int       width)
			double [] flatness = new double [tiles];
			for (int ntile = 0; ntile < disparity.length; ntile++) {
				flatness[ntile] = flatness_ref / (flatness_ref + Math.sqrt(curvature2[ntile]));
			}

			String [] titles = {
					"delta-X",       //  0
					"delta-Y",       //  1
					"conf",          //  2
					"disp",          //  3
					"disp-corr",     //  4 
					"ddisp",         //  5
					"dx/disp",       //  6
					"dy/disp",       //  7
					"sfm_gain",      //  8
					"sfm_gain_new",  //  9
					"sfm_gain_old",  // 10
					"pX",            // 11
					"pY",            // 12
					"reliable",      // 13
					"reliable-all",  // 14
					"reliable-new",  // 15
					"conf-reliable", // 16
					"conf-unreliable",// 17
					"flatness",       // 18
					"strength"        // 19
			};
			double [][] dbg_data = new double [titles.length][tiles];
			for (int i = 0; i < dbg_data.length; i++) {
				Arrays.fill(dbg_data[i], Double.NaN);
			}
			for (int ntile = 0; ntile < tiles; ntile++) {
				if (mvect[ntile] != null) {
					dbg_data[ 0][ntile] = mvect[ntile][0];
					dbg_data[ 1][ntile] = mvect[ntile][1];
					dbg_data[ 2][ntile] = mvect[ntile][2];
				}
				if (ref_pXpYD[ntile] != null) {
					dbg_data[ 3][ntile] = ref_pXpYD[ntile][2];
					dbg_data[11][ntile] = ref_pXpYD[ntile][0];
					dbg_data[12][ntile] = ref_pXpYD[ntile][1];
				}
				if (dpXYddisp[ntile] != null) {
					dbg_data[ 6][ntile] = dpXYddisp[ntile][0];
					dbg_data[ 7][ntile] = dpXYddisp[ntile][1];
				}
				dbg_data[13][ntile] = reliable[ntile]? 1.0 : 0.0;
				dbg_data[14][ntile] = reliable_all[ntile]? 1.0 : 0.0;
				if (reliable_new != null) {
					dbg_data[15][ntile] = reliable_new[ntile]? 1.0 : 0.0;
				}

				dbg_data[ 5][ntile] = ddisp[ntile];
				if ((ref_pXpYD[ntile] != null) && (dpXYddisp[ntile] != null)) {
					dbg_data[ 4][ntile] = ref_pXpYD[ntile][2] - ddisp[ntile];
				}
				dbg_data[ 8][ntile] = Math.sqrt(sfm_gain2[ntile]);
				dbg_data[ 9][ntile] = Math.sqrt(sfm_gain2_new[ntile]);
				dbg_data[10][ntile] = Math.sqrt(sfm_gain2_old[ntile]);

				dbg_data[16][ntile] = ( reliable[ntile] && (mvect[ntile] != null) )? mvect[ntile][2] : Double.NaN;  
				dbg_data[17][ntile] = (!reliable[ntile] && (mvect[ntile] != null) )? mvect[ntile][2] : Double.NaN;  
				dbg_data[18][ntile] = flatness[ntile];
			}
			dbg_data[18] = quadCLTs[ref_index].getDLS()[2];

			ShowDoubleFloatArrays.showArrays( // out of boundary 15
					dbg_data,
					tilesX,
					tilesY,
					true,
					scenes[0].getImageName()+"-"+scenes[1].getImageName()+"-test_sfm",
					titles);
		}
		if (reliable_new != null) {
			for (int ntile = 0; ntile < reliable_new.length; ntile++) {
				reliable[ntile] |= reliable_new[ntile];
			}
		}
		if (test_nan) {
			for (int i = 0; i < sfm_gain2.length; i++) {
				if (Double.isNaN(sfm_confidence[i])) {
					System.out.println("adjustDisparitySFM() end: sfm_confidence["+i+"] = null");
				}
				if (Double.isNaN(sfm_gain2[i])) {
					System.out.println("adjustDisparitySFM() end: sfm_gain2["+i+"] = null");
				}
				if (Double.isNaN(accum_weights[i])) {
					System.out.println("adjustDisparitySFM() end: accum_weights["+i+"] = null");
				}
			}
		}

		return true;
	}
	//TODO: use confidence from SFM (copied from MV confidence when used for largest sfm gain) as it is now available
	public static boolean FilterDisparitySFM(
			final CLTParameters           clt_parameters,
			// not used in this version, maybe it will later
			final boolean                 final_filter, // true after all pose adjustments are finished, false at intermediate steps
			final QuadCLT[]           	  quadCLTs,
			final int                     ref_index,
			final boolean []              reliable, // reliable's disparity only updated from other reliable, non-reliable - from all
			final int                     scene_index, // for debug, null _ final
			final boolean                 batch_mode,
			final int                     debugLevel) {
		boolean gen_img = debugLevel > 1000;
		boolean test_nan = debugLevel < 1000;
		final int dbg_tile = 22+47*80; // ( 7+23*80);
		final boolean use_lma_dsi =     clt_parameters.imp.use_lma_dsi;
		final int      filt_iters =     clt_parameters.imp.sfma_filt_iters;   // 10;     // Maximal number of filtering iteration
		final int      filt_nb_int =    clt_parameters.imp.sfma_filt_nb_int;  // 6;      // minimal number of defined (non-NaN) neighbors for strong internal tiles (excludes 2 columns/rows on each margin)
		final int      filt_nb_edge =   clt_parameters.imp.sfma_filt_nb_edge; // 4;      // minimal number of defined (non-NaN) neighbors for tiles on the edge of internals (exclude 1 row/col, edges - not neibs)
		final int      filt_nb_corn =   clt_parameters.imp.sfma_filt_nb_corn; // 3;      // minimal number of defined (non-NaN) neighbors for tiles in the corners internals (exclude 1 row/col, edges - not neibs)
		final double   filt_nsigma =    clt_parameters.imp.sfma_filt_nsigma;  // 3.0;    // tile should differ by more than scaled sigma of the counted neighbors
		final boolean  filt_mm =        clt_parameters.imp.sfma_filt_mm;      // false;  // Filter disparity if it is between min and max of defined neighbors
		final boolean  filt_weakbg =    clt_parameters.imp.sfma_filt_weakbg;  // true;   // Consider weak tiles to belong to bg if the next are met
		final double   filt_fgbg =      clt_parameters.imp.sfma_filt_fgbg;    // 0.1;    // If weak tile's neighbors max-min exceeds this, and
///		final double   filt_str =       clt_parameters.imp.sfma_filt_str;     // 0.9;    // it is NaN or weaker that this fraction of the local maximum, consider it a bg and average with bg tiles only
		final int tilesX =  quadCLTs[ref_index].getTileProcessor().getTilesX();
		final int tilesY =  quadCLTs[ref_index].getTileProcessor().getTilesY();
		
		final double [] ref_disparity =      quadCLTs[ref_index].getDLS()[use_lma_dsi?1:0]; 
		final double [] ref_disparity_next = ref_disparity.clone();
		final int [] fneib_stats = getEdgeStatus(tilesX, tilesY);
		final boolean [] changed = new boolean[tilesX*tilesY];
//		final boolean second_min = true; // if old value was not lower than all neighbors, use second minimal disparity for the background, false - use the absolute minimum.
		final int min_remain = 2; // remove minimal and maximal if at least this number remains.
		final double [][] ref_dbg = gen_img? (new double[filt_iters+2][]) : null; // 0 - original, last - reliable

		if (test_nan) {
			double [] dbg_strength = quadCLTs[ref_index].getDLS()[2];
			for (int i = 0; i < ref_disparity.length; i++) {
				if (Double.isNaN(dbg_strength[i])) {
					System.out.println("FilterDisparitySFM(): dbg_strength["+i+"] = null");
				}
			}
		}
		
		if (ref_dbg != null) {
			ref_dbg[0] = ref_disparity.clone();
		}
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
		final AtomicInteger [] amods = new AtomicInteger[3];
		for (int i = 0; i < amods.length; i++) {
			amods[i] = new AtomicInteger(0);
		}
   		for (int niter = 0; niter < filt_iters; niter++) {
   			ai.set(0);
   			for (AtomicInteger am: amods) am.set(0);
   			for (int ithread = 0; ithread < threads.length; ithread++) {
   				threads[ithread] = new Thread() {
   					public void run() {
   						TileNeibs tn = new TileNeibs(tilesX,tilesY);
   						int [] neib_stat = fneib_stats.clone();
   						for (int nTile = ai.getAndIncrement(); nTile < ref_disparity.length; nTile = ai.getAndIncrement()) {
   							if (nTile== dbg_tile) {
   							}
//   							boolean center_reliable = reliable[nTile];
   							//if (final_filter || (reliable[nTile]))
   							{ // before final only filter calculated
								int inner_type = neib_stat[nTile]; 
   								int min_type =  MIN_TYPE[inner_type]; // minimal type for the same distance from the edge
   								boolean is_edge = (inner_type - min_type) == 1; 
   								boolean is_corner = inner_type == min_type; 
   								int num_neibs = 0;
   								double sum_neibs = 0, sum_neibs2 = 0, min_neibs = Double.NaN, max_neibs = Double.NaN;
   								int [] outliers_dirs = {-1,-1}; // direction of the minimal and maximal disparity (to find the second min/max)
   								for (int dir = 0; dir < 8; dir++) {
   									int ntile1 = tn.getNeibIndex(nTile, dir);
   									if ((ntile1 >= 0) && (neib_stat[ntile1] >= min_type)) {
   										double d =  ref_disparity[ntile1];
   										/*  for unreliable center - any, for reliable - only reliable  */
   										if (!Double.isNaN(d) && (!reliable[nTile] || reliable[ntile1])) {
   											num_neibs++;
   											sum_neibs += d;
   											sum_neibs2 += d*d;
   											if (!(d >= min_neibs)) {
   												min_neibs = d;
   												outliers_dirs[0] = dir;
   											}
   											if (!(d <= max_neibs)) {
   												max_neibs = d;
   												outliers_dirs[1] = dir;
   											}
   										}
   									}
   								}
   								// TODO: optimize (first copy to double[8], maybe sort
   								if ((num_neibs >= filt_nb_int) || (is_edge && (num_neibs >= filt_nb_edge)) || (is_corner && (num_neibs >= filt_nb_corn))){
   									if (num_neibs >= (min_remain + 2)) { // re-run, removing out
   										num_neibs = 0;
   										sum_neibs = 0;
   										sum_neibs2 = 0;
   										min_neibs = Double.NaN;
   										max_neibs = Double.NaN;
   		   								for (int dir = 0; dir < 8; dir++) {
   		   									int ntile1 = tn.getNeibIndex(nTile, dir);
   		   									if ((ntile1 >= 0) && (neib_stat[ntile1] >= min_type) && (dir != outliers_dirs[0]) && (dir != outliers_dirs[1])) {
   		   										double d =  ref_disparity[ntile1];
   		   										/*  for unreliable center - any, for reliable - only reliable  */
   		   										if (!Double.isNaN(d) && (!reliable[nTile] || reliable[ntile1])) {
   		   											num_neibs++;
   		   											sum_neibs += d;
   		   											sum_neibs2 += d*d;
   		   											if (!(d >= min_neibs)) {
   		   												min_neibs = d;
   		   											}
   		   											if (!(d <= max_neibs)) {
   		   												max_neibs = d;
   		   											}
   		   										}
   		   									}
   		   								}
   									}
   									
   									// conditions met to modify center tile
   									double avg_disp = sum_neibs/num_neibs;
   									double sigma = Math.sqrt((sum_neibs2- sum_neibs* avg_disp)/(sum_neibs-1));
   									double old_d = ref_disparity[nTile];
   									if (Double.isNaN(old_d) || // always OK to replace undefined
   											(((old_d > max_neibs) || (old_d < min_neibs) || filt_mm) && // outside of min/max or allowed
   													(Math.abs(old_d - avg_disp) > filt_nsigma * sigma)) ) { // differs from the average by more than scaled sigma
   										double new_d = avg_disp;
   										// is it a background tile? NOT using any strengths for now
   										if (filt_weakbg && ((max_neibs-min_neibs) > filt_fgbg)) {
   											new_d = min_neibs;
   											/*
   											if (second_min && (new_d < old_d)) {
   												double min2_neibs = max_neibs;
   												for (int dir = 0; dir < 8; dir++) if (dir != lowest_dir){
   													int ntile1 = tn.getNeibIndex(nTile, dir);
   													if ((ntile1 >= 0) && (neib_stat[ntile1] >= min_type)) {
   														double d =  ref_disparity[ntile1];
   														if (!Double.isNaN(d)) {
   															if (!(d >= min2_neibs)) min2_neibs = d;

   														}
   													}
   												}
   												new_d = min2_neibs;
   											}
   											*/
   										}
   										ref_disparity_next[nTile] = new_d;
   										if (!changed[nTile]) { // only count changes to the new tiles
   											int aindx = Math.min(INNER_DIST[inner_type], amods.length-1);
   											amods[aindx].getAndIncrement();
   										}
   										changed[nTile] = true;
   									}
   								}
   							}
   						}
   					}
   				};
   			}		      
   			ImageDtt.startAndJoin(threads);
   			int total_mod = 0;
   			for (int i = 0; i < amods.length; i++) {
   				total_mod += amods[i].get();
   			}
    		if (debugLevel > -3) {
    			System.out.print("niter(): changed "+total_mod+", including from edge: ");
    			for (int i = 0; i < amods.length; i++) {
    				System.out.print(i+": "+amods[i].get()+", ");
    			}
    			System.out.println();
    		}
    		System.arraycopy(
    				ref_disparity_next,
    				0,
    				ref_disparity,
    				0,
    				ref_disparity.length);
			if (ref_dbg != null) { // [0] = ref_disparity.clone();
				ref_dbg[niter + 1] = ref_disparity.clone();
			}
			if (total_mod == 0) {
				break;
			}
   		}
   		if (ref_dbg != null) {
   			String [] titles = new String [ref_dbg.length];
   			titles[0] = "original";
   			for (int i = 1; i < ref_dbg.length-1; i++) {
   				titles[i]="iter-"+(i-1);
   			}
   			titles[ref_dbg.length-1] = "reliable";
   			ref_dbg[ref_dbg.length-1] = new double[tilesX*tilesY];
   			for (int i = 0; i < reliable.length; i++) {
   				ref_dbg[ref_dbg.length-1][i] = reliable[i] ? 100:Double.NaN; 
   			}
   			String title = quadCLTs[ref_index].getImageName();
   			if (scene_index >= 0) {
   				title += "-"+quadCLTs[scene_index].getImageName();
   			}
   			title += "filter-SFM";
   			ShowDoubleFloatArrays.showArrays( // out of boundary 15
					ref_dbg,
					tilesX,
					tilesY,
					true,
					title,
					titles);
   		}
		return true;
	}
	
	/**
	 * Get squared difference between each data element and average of its 8 immediate neighbors
	 * @param data 2d data as a 1d array in a linescan order
	 * @param width data width (height = data.length/width)
	 * @return array with the same dimensions as data containing squared differences between the center
	 * element and average of its neighbors
	 */
	public static double [] getSquaredCurvature(
			final double [] data,
			final int       width) {
		final int height = data.length/width;
		final double [] curv = new double [data.length];
		final double diag_w = 0.7;
		final double [] neib_w = {1.0,diag_w,1.0,diag_w,1.0,diag_w,1.0,diag_w}; 
		Arrays.fill(curv, Double.NaN);
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					TileNeibs tn = new TileNeibs(width,height);
					for (int nTile = ai.getAndIncrement(); nTile < data.length; nTile = ai.getAndIncrement()) {
						double d_cent = data[nTile];
						if (!Double.isNaN(d_cent)) {
							double sumw = 0, sumwd = 0.0;
							for (int dir = 0; dir < 8; dir++) {
								int neib = tn.getNeibIndex(nTile, dir);
								if ((neib >= 0) && !Double.isNaN(data[neib])) {
									sumw += neib_w[dir];
									sumwd += neib_w[dir] * data[neib];
								}
							}
							if (sumw > 0) {
								double diff = d_cent - sumwd/sumw;
								curv[nTile] = diff * diff;
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return curv;
	}
	
	public static int [] getEdgeStatus(
			int tilesX,
			int tilesY) {
		final int [] edge_stat = new int[tilesX * tilesY];
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					TileNeibs tn = new TileNeibs(tilesX,tilesY);
					Rectangle rect_inner = tn.getInnerRectangle();
					Rectangle rect_strong_inner = tn.getStrongInnerRectangle();
					for (int nTile = ai.getAndIncrement(); nTile < edge_stat.length; nTile = ai.getAndIncrement()) {
						Point p = tn.getPoint(nTile);
						if (rect_strong_inner.contains(p)) {
							int nn = 0;
							for (int dir = 0; dir < 8; dir++) {
								if (rect_strong_inner.contains(tn.getPoint(tn.getNeibIndex(nTile, dir)))) nn++;
							}
							switch (nn) {
							case 8: edge_stat[nTile] = TILE_STRONG_INNER;  break;
							case 5: edge_stat[nTile] = TILE_STRONG_EDGE;   break;
							case 3: edge_stat[nTile] = TILE_STRONG_CORNER; break;
							default:
								throw new IllegalArgumentException ("getEdgeStatus() 1: invalid neighbors: "+nn);
							}
						} else if (rect_inner.contains(p)) {
							int nn = 0;
							for (int dir = 0; dir < 8; dir++) {
								if (rect_inner.contains(tn.getPoint(tn.getNeibIndex(nTile, dir)))) nn++;
							}
							switch (nn) {
							case 5: edge_stat[nTile] = TILE_INNER_EDGE;   break;
							case 3: edge_stat[nTile] = TILE_INNER_CORNER; break;
							default:
								throw new IllegalArgumentException ("getEdgeStatus() 2: invalid neighbors: "+nn);
							}
						} else { // outer edge
							int nn = 0;
							for (int dir = 0; dir < 8; dir++) {
								int ntile1 = tn.getNeibIndex(nTile, dir);
								if (tn.getNeibIndex(nTile, dir) >=0) nn++;
							}
							switch (nn) {
							case 5: edge_stat[nTile] = TILE_EDGE;   break;
							case 3: edge_stat[nTile] = TILE_CORNER; break;
							default:
								throw new IllegalArgumentException ("getEdgeStatus() 3: invalid neighbors: "+nn);
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return edge_stat;
	}

}
