package com.elphel.imagej.tileprocessor;

import java.util.Arrays;
import java.util.concurrent.atomic.AtomicInteger;

import org.apache.commons.math3.geometry.euclidean.threed.Rotation;

import com.elphel.imagej.calibration.CalibrationFileManagement;
import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.cameras.ColorProcParameters;
import com.elphel.imagej.common.DoubleGaussianBlur;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.gpu.GpuQuad;
import com.elphel.imagej.gpu.TpTask;
import com.elphel.imagej.ims.Did_ins_1;
import com.elphel.imagej.ims.Did_ins_2;
import com.elphel.imagej.ims.Did_pimu;
import com.elphel.imagej.ims.Imx5;

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

public class Interscene {
	public final OpticalFlow opticalFlow;
	// interscene adjustments failure reasons.
	public static final int FAIL_REASON_LMA =            1; // LMA failed
	public static final int FAIL_REASON_INTERSCENE =     2; // clt_process_tl_interscene() returned null
	public static final int FAIL_REASON_MIN =            3; // average pixel offset is below specified threshold (FPN)
	public static final int FAIL_REASON_MAX =            4; // average pixel offset is above specified threshold (overlap)
	public static final int FAIL_REASON_NULL =           5; // null offsets array
	public static final int FAIL_REASON_EMPTY =          6; // No offset pairs in offsets array
	public static final int FAIL_REASON_ROLL =           7; // Too high roll between the images
	public static final int FAIL_REASON_ZOOM =           8; // Too high zoom ratio between the images
	
	public static String getFailReason(int fr) {
		switch (fr) {
		case FAIL_REASON_LMA:        return "FAIL_REASON_LMA";
		case FAIL_REASON_INTERSCENE: return "FAIL_REASON_INTERSCENE";
		case FAIL_REASON_MIN:        return "FAIL_REASON_MIN";
		case FAIL_REASON_MAX:        return "FAIL_REASON_MAX";
		case FAIL_REASON_NULL:       return "FAIL_REASON_NULL";
		case FAIL_REASON_EMPTY:      return "FAIL_REASON_EMPTY";
		case FAIL_REASON_ROLL:       return "FAIL_REASON_ROLL";
		case FAIL_REASON_ZOOM:       return "FAIL_REASON_ZOOM";
		default:
			return "unknown scene-matching failure reason="+fr;
		}
	}
	
	public static double [] ZERO3 = {0.0,0.0,0.0};
//	public static double  LINE_ERR = 0.1;
	public static int     THREADS_MAX =          100;  // maximal number of threads to launch
	
	public Interscene (OpticalFlow opticalFlow) {
		this.opticalFlow = opticalFlow;
	}

	
	public static boolean setInitialOrientations(
			final CLTParameters          clt_parameters,
			final ColorProcParameters    colorProcParameters,
			final QuadCLT[]              quadCLTs, //
			final int                    ref_index,
			final QuadCLT.SetChannels [] set_channels,
			final boolean                batch_mode,
			int                          earliest_scene,
			int []                       start_ref_pointers, // [0] - earliest valid scene, [1] ref_index
			final int                    threadsMax,  // int               threadsMax,
			final boolean                updateStatus,
			final int                    debugLevel) {
		int num_scene_retries = 10;
		int retry_radius =      2;
		double  scale_extrap_atr =       clt_parameters.imp.scale_extrap_atr;
		double  scale_extrap_xyz =       clt_parameters.imp.scale_extrap_xyz;
		int     avg_len	=                clt_parameters.imp.avg_len;
		double maximal_series_rms = 0.0;
		double  min_ref_str =       clt_parameters.imp.min_ref_str;
		double  min_ref_frac=       clt_parameters.imp.min_ref_frac;		
    	int min_num_scenes =        clt_parameters.imp.min_num_scenes; // abandon series if there are less than this number of scenes in it 
    	int max_num_scenes =        clt_parameters.imp.max_num_scenes; // cut longer series
    	
    	double boost_max_short = 2.0; // 
    	double boost_zoom_short = 1.5; // 
    	
    	
    	
		double [] lma_rms = new double[2];
		double [] use_atr = null;

		int tilesX =  quadCLTs[ref_index].getTileProcessor().getTilesX();
//        int tilesY =  quadCLTs[ref_index].getTileProcessor().getTilesY();
        int tile_size = quadCLTs[ref_index].getTileProcessor().getTileSize();
		double min_offset =         0.0; // clt_parameters.imp.min_offset;
		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 fpn_skip =          clt_parameters.imp.fpn_skip; // if false - fail as before
		boolean fpn_rematch =       clt_parameters.imp.fpn_rematch; // if false - keep previous
		double [] min_max = {min_offset, max_offset, 0.0} ; // {min, max, actual rms)
		int []    fail_reason = new int[1];   // null or int[1]: 0 - OK, 2 - LMA, 3 - min, 4 - max
		for (int scene_index =  ref_index - 1; scene_index >= 0 ; scene_index--) {
			// to include ref scene photometric calibration
			quadCLTs[scene_index] = quadCLTs[ref_index].spawnNoModelQuadCLT(
					set_channels[scene_index].set_name,
					clt_parameters,
					colorProcParameters, //
					threadsMax,
					debugLevel-2);
		} // split cycles to remove output clutter
		ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection();		
		int debug_scene = -15;
		boolean debug2 = !batch_mode; // false; // true;
		boolean [] reliable_ref = null;
		boolean use_lma_dsi =      clt_parameters.imp.use_lma_dsi;
        double [] reduced_strength = new double[1];
		if (min_ref_str > 0.0) {
			reliable_ref = quadCLTs[ref_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,
					true,         // boolean needs_lma);
					reduced_strength); // if not null will return >0 if had to reduce strength (no change if did not reduce)
			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[ref_index].getTileProcessor().getTilesX(),
						quadCLTs[ref_index].getTileProcessor().getTilesY(),
						"reliable_ref");
			}
		}
		
		double max_z_change = Double.NaN; // only applicable for drone images
		if (max_zoom_diff > 0) { // ignore if set to
			double avg_z = quadCLTs[ref_index].getAverageZ(true); // use lma
			max_z_change = avg_z * max_zoom_diff;
			if (debugLevel > -3) {
				System.out.println("Setting maximal Z-direction movement to "+ max_z_change+" m.");
			}
		}

		
		double [][][] scenes_xyzatr =      new double [quadCLTs.length][][]; // previous scene relative to the next one
		scenes_xyzatr[ref_index] =         new double[2][3]; // all zeros
		boolean after_spiral = false;
		boolean got_spiral = false;
		int    search_rad =        clt_parameters.imp.search_rad;    // 10;
		for (int scene_index =  ref_index - 1; scene_index >= earliest_scene ; scene_index--) {
			if ((ref_index - scene_index) >= max_num_scenes){
				earliest_scene = scene_index + 1;
				if (debugLevel > -3) {
					System.out.println("Cutting too long series at scene "+scene_index+" (of "+ quadCLTs.length+". excluding) ");
				}
				// set this and all previous to null
				for (; scene_index >= 0 ; scene_index--) {
					ers_reference.addScene(quadCLTs[scene_index].getImageName(), null);
				}
				break;
			}
			if (scene_index == debug_scene) {
				System.out.println("scene_index = "+scene_index);
				System.out.println("scene_index = "+scene_index);
			}
			QuadCLT scene_QuadClt = quadCLTs[scene_index];
			// get initial xyzatr:
			// TODO:repeat spiralSearchATR if found scene was too close
//			if (scene_index ==  ref_index - 1) { // search around for the best fit
			if (!got_spiral) { // search around for the best fit
				use_atr = spiralSearchATR(
						clt_parameters, // CLTParameters            clt_parameters,
				    	use_lma_dsi,    // may be turned off by the caller if there are too few points
						search_rad,     // int    search_rad
						quadCLTs[ref_index], // QuadCLT             reference_QuadClt,
						scene_QuadClt, // QuadCLT                   scene_QuadClt,
						reliable_ref, // ********* boolean []                reliable_ref,
						debugLevel);
				if (use_atr == null) {
					if (num_scene_retries-- > 0) {
						search_rad = retry_radius; // faster, no need/sense to look far
						continue;
					}
					earliest_scene = scene_index + 1;
					if (debugLevel > -3) {
						System.out.println("Pass multi scene "+scene_index+" (of "+ quadCLTs.length+") "+
								quadCLTs[ref_index].getImageName() + "/" + scene_QuadClt.getImageName()+
								" spiralSearchATR() FAILED. Setting earliest_scene to "+earliest_scene);
					}
					// set this and all previous to null
					for (; scene_index >= 0 ; scene_index--) {
						if (quadCLTs[scene_index] == null) {
							System.out.println("setInitialOrientations(): quadCLTs["+scene_index+"] is alreadu null!");
						} else {
							ers_reference.addScene(quadCLTs[scene_index].getImageName(), null); // null pointer
						}
					}
					break; // failed with even first before reference
				}
				after_spiral = true;
				got_spiral = true;
				scenes_xyzatr[scene_index] = new double [][] {new double[3], use_atr};
			} else { // assume linear motion
				after_spiral = false;
				if (scene_index == debug_scene) {
					System.out.println("adjusting orientation, scene_index="+scene_index);
					System.out.println("adjusting orientation, scene_index="+scene_index);
				}
				int na = avg_len;
				if ((scene_index + 1 + na) > ref_index) {
					na = ref_index - (scene_index + 1); 
				}
				
				double [][] last_diff = ErsCorrection.combineXYZATR(
						scenes_xyzatr[scene_index + 1],
						ErsCorrection.invertXYZATR(scenes_xyzatr[scene_index+1 + na]));
				for (int i = 0; i < 3; i++) {
					last_diff[0][i] /= na;
					last_diff[1][i] /= na;
				}
				for (int i = 0; i < 3; i++) {
					last_diff[0][i] *= scale_extrap_xyz;
					last_diff[1][i] *= scale_extrap_atr;
				}					
				scenes_xyzatr[scene_index] = ErsCorrection.combineXYZATR(
						scenes_xyzatr[scene_index+1],
						last_diff);
			}
			// Refine with LMA
			double [][] initial_pose = new double[][]{
				scenes_xyzatr[scene_index][0].clone(),
				scenes_xyzatr[scene_index][1].clone()};
			boolean rot_to_transl = after_spiral && clt_parameters.ilp.ilma_translation_priority;
			double [] reg_weights = rot_to_transl? clt_parameters.ilp.ilma_regularization_weights0 : clt_parameters.ilp.ilma_regularization_weights;
			min_max[1] = max_offset;
			//boost_zoom_short
			double max_z_change_scaled = max_z_change;
			if ((ref_index - scene_index) < min_num_scenes) {
				min_max[1] = boost_max_short * max_offset;
				max_z_change_scaled = max_z_change * boost_zoom_short;
				if (debugLevel > -3) {
					System.out.println("As the current series ("+(ref_index - scene_index)+
							" scenes) is shorter than minimal ("+min_num_scenes+"), the maximal shift is scaled by "+
							boost_max_short + " to " + (boost_max_short * max_offset)+
							" pixels");
				}
			}
			if ((scene_index - earliest_scene) < min_num_scenes) {
				min_max[1] = boost_max_short * max_offset;
				max_z_change_scaled = max_z_change * boost_zoom_short;
				if (debugLevel > -3) {
					System.out.println("As the remaining number of scenes to process ("+(scene_index - earliest_scene + 1)+
							") is less than minimal ("+min_num_scenes+"), the maximal shift is scaled by "+
							boost_max_short + " to " + (boost_max_short * max_offset)+
							" pixels");
				}
			}
			
			scenes_xyzatr[scene_index] = adjustPairsLMAInterscene(
					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
					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[ref_index], // QuadCLT reference_QuadCLT,
					null,                // double []        ref_disparity, // null or alternative reference disparity
					reliable_ref,        // boolean []     reliable_ref, // null or bitmask of reliable reference tiles
					scene_QuadClt,                                  // QuadCLT scene_QuadCLT,
					initial_pose[0],        // xyz
					initial_pose[1],        // atr
					initial_pose[0],        // double []      scene_xyz_pull, // if both are not null, specify target values to pull to 
					rot_to_transl? (new double[3]):initial_pose[1],        // double []      scene_atr_pull, // 
					clt_parameters.ilp.ilma_lma_select,                                  // final boolean[]   param_select,
					reg_weights, // clt_parameters.ilp.ilma_regularization_weights,                              //  final double []   param_regweights,
					lma_rms,                                        // double []      rms, // null or double [2]
					clt_parameters.imp.max_rms,                     // double         max_rms,
					clt_parameters.imp.debug_level);                // 1); // -1); // int debug_level);

			boolean adjust_OK = scenes_xyzatr[scene_index] != null;
			if (adjust_OK) { // check only for initial orientation, do not check on readjustments
				if (Math.abs(scenes_xyzatr[scene_index][1][2]) > max_roll) {
					fail_reason[0] = FAIL_REASON_ROLL;
					adjust_OK = false;
				}
				if (max_zoom_diff > 0) { // ignore if set to 
					if (Math.abs(scenes_xyzatr[scene_index][0][2]) > max_z_change) {
						if (Math.abs(scenes_xyzatr[scene_index][0][2]) > max_z_change_scaled) {
							fail_reason[0] = FAIL_REASON_ZOOM;
							adjust_OK = false;
						} else {
							System.out.println("Z-change "+Math.abs(scenes_xyzatr[scene_index][0][2])+
							"m exceeds limit of "+max_z_change+"m, but as it is beginning/end of the series, limit is raised to "+
							max_z_change_scaled+"m");
						}
					}
				}
			}
			// FAIL_REASON_ROLL
			handle_failure: {
				if (!adjust_OK) {
//					boolean OK = false;
					System.out.println("LMA failed at nscene = "+scene_index+". Reason = "+fail_reason[0]+
							" ("+getFailReason(fail_reason[0])+")");
					if ((fail_reason[0]==FAIL_REASON_MIN) || ((fail_reason[0]==FAIL_REASON_LMA) && !got_spiral)) {
						if (fpn_skip) {
							System.out.println("fpn_skip is set, just using initial pose");
							scenes_xyzatr[scene_index] = initial_pose;
							break handle_failure; // to next scene
						} else {
							System.out.println("fpn_skip is not set, aborting series and adjusting earliest_scene");
							// set this and all previous to null
						}
					}
					// all other reasons lead to failure
					earliest_scene = scene_index + 1;
					if (debugLevel > -4) {
						System.out.println("Pass multi scene "+scene_index+" (of "+ quadCLTs.length+") "+
								quadCLTs[ref_index].getImageName() + "/" + scene_QuadClt.getImageName()+
								" FAILED. Setting earliest_scene to "+earliest_scene + " Failure reason: " + fail_reason[0]+
								" ("+getFailReason(fail_reason[0])+")");
					}
					// set this and all previous to null
					for (; scene_index >= 0 ; scene_index--) {
						ers_reference.addScene(quadCLTs[scene_index].getImageName(), null);
						//					quadCLTs[scene_index] = null; // completely remove early scenes?
					}
					break;
				}
			}
			if (after_spiral && (scene_index < (ref_index-1))) { // need to interpolate skipped scenes
				// here we expect very small translations/angles, so linear scaling is OK
				double s = 1.0 / (ref_index - scene_index);
				for (int interp_index = ref_index-1; interp_index > scene_index; interp_index--) {
					scenes_xyzatr[interp_index] = new double[2][3];
					for (int i = 0; i < 2; i++) {
						for (int j = 0; j < 3; j++) {
							scenes_xyzatr[interp_index][i][j] =
									scenes_xyzatr[scene_index][i][j] * s *(interp_index - scene_index); 
						}
					}
					ers_reference.addScene(scene_QuadClt.getImageName(),
							scenes_xyzatr[interp_index][0],
							scenes_xyzatr[interp_index][1],
							ZERO3, // ers_scene.getErsXYZ_dt(),		
							ZERO3 // ers_scene.getErsATR_dt()		
							);
				}
			}
			
			/* old version
			if (scenes_xyzatr[scene_index] == null) {
				earliest_scene = scene_index + 1;
				if (debugLevel > -3) {
					System.out.println("Pass multi scene "+scene_index+" (of "+ quadCLTs.length+") "+
							quadCLTs[ref_index].getImageName() + "/" + scene_QuadClt.getImageName()+
							" FAILED. Setting earliest_scene to "+earliest_scene);
				}
				// set this and all previous to null
				for (; scene_index >= 0 ; scene_index--) {
					ers_reference.addScene(quadCLTs[scene_index].getImageName(), null);
				}
				break;
			}
			*/
			// TODO: Maybe after testing high-ers scenes - restore velocities from before/after scenes
			ers_reference.addScene(scene_QuadClt.getImageName(),
					scenes_xyzatr[scene_index][0],
					scenes_xyzatr[scene_index][1],
					ZERO3, // ers_scene.getErsXYZ_dt(),		
					ZERO3 // ers_scene.getErsATR_dt()		
					);
		    if (lma_rms[0] > maximal_series_rms) {
		        maximal_series_rms = lma_rms[0];
		    }

			if (debugLevel > -3) {
				System.out.println("Pass multi scene "+scene_index+" (of "+ quadCLTs.length+") "+
						quadCLTs[ref_index].getImageName() + "/" + scene_QuadClt.getImageName()+
						" Done. RMS="+lma_rms[0]+", maximal so far was "+maximal_series_rms);
			}
		} // for (int scene_index =  ref_index - 1; scene_index >= 0 ; scene_index--)
		if ((ref_index - earliest_scene + 1) < min_num_scenes) {
			System.out.println("Total number of useful scenes = "+(ref_index - earliest_scene + 1)+
					" < "+min_num_scenes+". Scrapping this series.");
			if (start_ref_pointers != null) {
				start_ref_pointers[0] = earliest_scene;
			}
			return false;
		}
		if (earliest_scene > 0) {
			System.out.println("Not all scenes matched, earliest useful scene = "+earliest_scene+
					" (total number of scenes = "+(ref_index - earliest_scene + 1)+
					").Maximal RMSE was "+maximal_series_rms);
		} else if (debugLevel > -4) {
			System.out.println("All multi scene passes are Done. Maximal RMSE was "+maximal_series_rms);
		}
		quadCLTs[ref_index].set_orient(1); // first orientation
		quadCLTs[ref_index].set_accum(0);  // reset accumulations ("build_interscene") number
		quadCLTs[ref_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);
		return true;
	}
	
	public static int  reAdjustPairsLMAInterscene( // after combo dgi is available and preliminary poses are known
			CLTParameters  clt_parameters,
			double         mb_max_gain,
			boolean []     reliable_ref, // null or bitmask of reliable reference tiles			
    		QuadCLT []     quadCLTs,
    		boolean        test_motion_blur,
			int            debugLevel)
	{ 
		System.out.println("reAdjustPairsLMAInterscene(): using mb_max_gain="+mb_max_gain);
		boolean fail_on_zoom_roll=false; // it should fail on initial adjustment
		int     avg_len	=                clt_parameters.imp.avg_len;
//		boolean test_motion_blur = true;//false
        // Set up velocities from known coordinates, use averaging
        double  half_run_range =    clt_parameters.ilp.ilma_motion_filter; // 3.50; // make a parameter
		double [][] dbg_scale_dt = {clt_parameters.ilp.ilma_scale_xyz, clt_parameters.ilp.ilma_scale_atr};
		boolean debug_ers =         clt_parameters.ilp.ilma_debug_ers ; // true;
		
		String dbg_ers_string = 
				((dbg_scale_dt[1][0] > 0)?"a":((dbg_scale_dt[1][0] < 0) ? "A":"0"))+
				((dbg_scale_dt[1][1] > 0)?"t":((dbg_scale_dt[1][1] < 0) ? "T":"0"))+
				((dbg_scale_dt[1][2] > 0)?"r":((dbg_scale_dt[1][2] < 0) ? "R":"0"))+
				((dbg_scale_dt[0][0] > 0)?"x":((dbg_scale_dt[0][0] < 0) ? "X":"0"))+
				((dbg_scale_dt[0][1] > 0)?"y":((dbg_scale_dt[0][1] < 0) ? "Y":"0"))+
				((dbg_scale_dt[0][2] > 0)?"z":((dbg_scale_dt[0][2] < 0) ? "Z":"0"));
		
//		boolean ers_readjust_enable = true; // debugging ers during readjust 11.02.2022
//		boolean ers_use_xyz =      false; //    true; // false; // enable when ready            11.02.2022
//		int [][] dbg_scale_dt = {{0,0,0},{0, 0, 0}}; // no_ers - causes singular matrices (not alwasy), but works
//		int [][] dbg_scale_dt = {{0,0,0},{1,-1, 1}}; // aTr 
//		int [][] dbg_scale_dt = {{0,0,0},{1, 1,-1}}; // atR 
//		int [][] dbg_scale_dt = {{0,0,0},{1, 1, 1}}; // atr
//		int [][] dbg_scale_dt = {{1,1,1},{1, 1, 1}}; // atrxyz 
//		int [][] dbg_scale_dt = {{1,1,1},{1, 1,-1}}; // atRxyz 
//		int [][] dbg_scale_dt = {{-1,-1,-1},{1, 1,1}}; // atrXYZ
//		int [][] dbg_scale_dt = {{-1,-1,-1},{1, 1,-1}}; // atRXYZ
		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
		int     margin =      clt_parameters.imp.margin;
        double second_margin = 1.1; // allow slightly larger offset for readjsutments (to be limited by the first pass)
        double boost_max_short = 5.0; // 2.0; // Some bug?
		int earliest_scene = 0;
	    boolean use_combo_dsi =        clt_parameters.imp.use_combo_dsi;
	    boolean use_lma_dsi =          clt_parameters.imp.use_lma_dsi;
    	int ref_index = quadCLTs.length-1;
		int tilesX =  quadCLTs[ref_index].getTileProcessor().getTilesX();
        int tilesY =  quadCLTs[ref_index].getTileProcessor().getTilesY();
        int tile_size = quadCLTs[ref_index].getTileProcessor().getTileSize();
		double min_offset =         clt_parameters.imp.min_offset;
		double max_offset =         boost_max_short*second_margin*clt_parameters.imp.max_rel_offset * tilesX * tile_size;
		double max_roll =           second_margin*clt_parameters.imp.max_roll_deg*Math.PI/180.0;
		double max_zoom_diff =      second_margin*clt_parameters.imp.max_zoom_diff;
		boolean fpn_skip =          clt_parameters.imp.fpn_skip; // if false - fail as before
		boolean fpn_rematch =       clt_parameters.imp.fpn_rematch; // if false - keep previous
		double [] min_max = {min_offset, max_offset, 0.0} ; // {min, max, actual rms)
		int []    fail_reason = new int[1];   // null or int[1]: 0 - OK, 2 - LMA, 3 - min, 4 - max
		System.out.println("reAdjustPairsLMAInterscene(): min_offset="+min_max[0]+", max_offset="+min_max[1]);
        double [] disparity_raw = new double [tilesX * tilesY];
        Arrays.fill(disparity_raw,clt_parameters.disparity);
///        double [][] combo_dsn_final = quadCLTs[ref_index].readDoubleArrayFromModelDirectory(
///        			"-INTER-INTRA-LMA", // String      suffix,
///        			0, // int         num_slices, // (0 - all)
///        			null); // int []      wh);
        double [][] combo_dsn_final = quadCLTs[ref_index].restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky

        double [][] dls = { // Update to use FG? Or FG/no BG?
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP],
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA],
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_STRENGTH]
        };
        double [][] ds = OpticalFlow.conditionInitialDS(
        		true,                // boolean        use_conf,       // use configuration parameters, false - use following  
        		clt_parameters,      // CLTParameters  clt_parameters,
        		dls,                 // double [][]    dls
        		quadCLTs[ref_index], // QuadCLT        scene,
        		debugLevel);			
//        double [] disparity_fg = ds[0]; // combo_dsn_final[COMBO_DSN_INDX_DISP_FG];
        double [] interscene_ref_disparity = null; // keep null to use old single-scene disparity for interscene matching
        if (use_combo_dsi) {
        	interscene_ref_disparity = ds[0].clone(); // use_lma_dsi ?
        	if (use_lma_dsi) {
        		for (int i = 0; i < interscene_ref_disparity.length; i++) {
        			if (Double.isNaN(dls[1][i])) {
        				interscene_ref_disparity[i] = Double.NaN;
        			}
        		}
        	}
        }
        
        double [][] ref_pXpYD = null;
        double [][] dbg_mb_img = null;
        double [] mb_ref_disparity =null;
        //        if (test_motion_blur) {
        mb_ref_disparity = interscene_ref_disparity;
        if (mb_ref_disparity == null) {
        	mb_ref_disparity = quadCLTs[ref_index].getDLS()[use_lma_dsi?1:0];
        }
        // ref_pXpYD should correspond to uniform grid (do not undo ERS of the reference scene)
        ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
        		null, // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
        		mb_ref_disparity, // dls[0],  // final double []   disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
        		ZERO3,         // final double []   scene_xyz, // camera center in world coordinates
        		ZERO3,         // final double []   scene_atr, // camera orientation relative to world frame
        		quadCLTs[ref_index],     // final QuadCLT     scene_QuadClt,
        		quadCLTs[ref_index]);    // final QuadCLT     reference_QuadClt)
        if (test_motion_blur) {
        	dbg_mb_img = new double[quadCLTs.length][];
        }
        
        
		ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection();
		double [][][] scenes_xyzatr = new double [quadCLTs.length][][]; // previous scene relative to the next one
		
		scenes_xyzatr[ref_index] = new double[2][3]; // all zeros
		// should have at least next or previous non-null
		int debug_scene = -8;
		double maximal_series_rms = 0.0;
		double [][] mb_vectors_ref = null;
		TpTask[][] tp_tasks_ref = null;
    	if (debug_ers) {
    		System.out.println("ERS velocities scale mode = '"+dbg_ers_string+"'");
    	}
        for (int nscene = ref_index; nscene >= earliest_scene; nscene--) {
			if (nscene == debug_scene) {
				System.out.println("nscene = "+nscene);
				System.out.println("nscene = "+nscene);
			}
			// just checking it is not isolated
        	if (quadCLTs[nscene] == null) {
        		earliest_scene = nscene + 1;
        		break;
        	}
        	if (nscene != ref_index) {
        		String ts = quadCLTs[nscene].getImageName();
        		if ((ers_reference.getSceneXYZ(ts)== null) || (ers_reference.getSceneATR(ts)== null)) {
        			earliest_scene = nscene + 1;
        			break;
        		}
        		scenes_xyzatr[nscene] = new double[][] {ers_reference.getSceneXYZ(ts), ers_reference.getSceneATR(ts)};
        	}
        	if (debug_ers) {
        		System.out.println(String.format("%3d xyz:    %9.5f %9.5f %9.5f   atr:    %9.5f %9.5f %9.5f",
        				nscene,
        				scenes_xyzatr[nscene][0][0], scenes_xyzatr[nscene][0][1], scenes_xyzatr[nscene][0][2],
        				scenes_xyzatr[nscene][1][0], scenes_xyzatr[nscene][1][1], scenes_xyzatr[nscene][1][2]));
        	}
        }
        double [][][] dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
        		quadCLTs,        // QuadCLT []     scenes, // ordered by increasing timestamps
        		ref_index,
        		earliest_scene,  // int            start_scene,
        		ref_index,       // int            end1_scene,
        		scenes_xyzatr,   // double [][][]  scenes_xyzatr,
        		half_run_range); // double         half_run_range
        
    	if (debug_ers) {
    		System.out.println();
    		for (int nscene = ref_index; nscene >= earliest_scene; nscene--) {
    		System.out.println(String.format("%3d xyz_dt: %9.5f %9.5f %9.5f   atr_dt: %9.5f %9.5f %9.5f",
    				nscene,
    				dxyzatr_dt[nscene][0][0], dxyzatr_dt[nscene][0][1], dxyzatr_dt[nscene][0][2],
    				dxyzatr_dt[nscene][1][0], dxyzatr_dt[nscene][1][1], dxyzatr_dt[nscene][1][2]));
    		}
    		System.out.println();
    	}
		double max_z_change = Double.NaN; // only applicable for drone images
		if (max_zoom_diff > 0) { // ignore if set to
			double avg_z = quadCLTs[ref_index].getAverageZ(true); // use lma
			max_z_change = avg_z * max_zoom_diff;
			if (fail_on_zoom_roll) {
				if (debugLevel > -3) {
					System.out.println("Setting maximal Z-direction movement to "+ max_z_change+" m.");
				}
			}
		}
    	
    	boolean [] failed_scenes = new boolean[quadCLTs.length];
    	int num_failed =0;
        for (int nscene = ref_index; nscene >= earliest_scene; nscene--) {
			if (nscene == debug_scene) {
				System.out.println("nscene = "+nscene);
				System.out.println("nscene = "+nscene);
			}
			// just checking it is not isolated
        	if ((quadCLTs[nscene] == null) ||
//        			(scenes_xyzatr[nscene] == null) || // no velocity even with averaging
        			((nscene != ref_index) &&
        			((ers_reference.getSceneXYZ(quadCLTs[nscene].getImageName())== null) ||
        			(ers_reference.getSceneATR(quadCLTs[nscene].getImageName())== null)))) {
        		earliest_scene = nscene + 1;
        		break;
        	}
			String ts = quadCLTs[nscene].getImageName();
    		
			double []   scene_xyz_pre = ZERO3;
			double []   scene_atr_pre = ZERO3;
			// find correct signs when setting. dxyzatr_dt[][] is also used for motion blur (correctly)
			
			double [][] scaled_dxyzatr_dt = new double[2][3];
			for (int i = 0; i < scaled_dxyzatr_dt.length; i++) {
				for (int j = 0; j < scaled_dxyzatr_dt[i].length; j++) {
					scaled_dxyzatr_dt[i][j] = dxyzatr_dt[nscene][i][j]* dbg_scale_dt[i][j];
				}
			}
			quadCLTs[nscene].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
					scaled_dxyzatr_dt[0], // (ers_use_xyz? dxyzatr_dt[nscene][0]: ZERO3), //, // dxyzatr_dt[nscene][0], // double []    ers_xyz_dt,
					scaled_dxyzatr_dt[1]); // dxyzatr_dt[nscene][1]); // double []    ers_atr_dt)(ers_scene_original_xyz_dt);
			if (dbg_mb_img != null) {
				boolean show_corrected = false;
				if (nscene == debug_scene) {
					System.out.println("2.nscene = "+nscene);
					System.out.println("3.nscene = "+nscene);
				}
				dbg_mb_img[nscene] = new double [tilesX*tilesY*2];
				Arrays.fill(dbg_mb_img[nscene],Double.NaN);
				double [] mb_scene_xyz = (nscene != ref_index)? ers_reference.getSceneXYZ(ts):ZERO3;
				double [] mb_scene_atr = (nscene != ref_index)? ers_reference.getSceneATR(ts):ZERO3;
				double [][] motion_blur = OpticalFlow.getMotionBlur( // dxyzatr_dt[][] should not be scaled here
						quadCLTs[ref_index],   // QuadCLT        ref_scene,
						quadCLTs[nscene],      // QuadCLT        scene,         // can be the same as ref_scene
						ref_pXpYD,             // double [][]    ref_pXpYD,     // here it is scene, not reference!
						mb_scene_xyz,          // double []      camera_xyz,
						mb_scene_atr,          // double []      camera_atr,
						dxyzatr_dt[nscene][0], // double []      camera_xyz_dt,
						dxyzatr_dt[nscene][1], // double []      camera_atr_dt,
						-1,                    // int            shrink_gaps,  // will gaps, but not more that grow by this
						debugLevel); // int            debug_level)
				for (int nTile = 0; nTile < motion_blur[0].length; nTile++) {
					int tx = nTile % tilesX;
					int ty = nTile / tilesX;
					dbg_mb_img[nscene][tx + tilesX * (ty*2 +0)] = mb_tau * motion_blur[0][nTile];
					dbg_mb_img[nscene][tx + tilesX * (ty*2 +1)] = mb_tau * motion_blur[1][nTile];
				}
				while (show_corrected) {
					ImagePlus imp_mbc = QuadCLT.renderGPUFromDSI(
							-1,                  // final int         sensor_mask,
							false,               // final boolean     merge_channels,
							null,                // final Rectangle   full_woi_in,      // show larger than sensor WOI (or null)
							clt_parameters,      // CLTParameters     clt_parameters,
							mb_ref_disparity,    // double []         disparity_ref,
							// motion blur compensation 
							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
							motion_blur,         // double [][]       mb_vectors,  //
							
							mb_scene_xyz,        // ZERO3,               // final double []   scene_xyz, // camera center in world coordinates
							mb_scene_atr,        // final double []   scene_atr, // camera orientation relative to world frame
							quadCLTs[nscene],    // final QuadCLT     scene,
							quadCLTs[ref_index], // final QuadCLT     ref_scene, // now - may be null - for testing if scene is rotated ref
							false, // toRGB,               // final boolean     toRGB,
							clt_parameters.imp.show_color_nan,
							quadCLTs[nscene].getImageName()+"-MOTION_BLUR_CORRECTED", // String            suffix,
							THREADS_MAX,          // int               threadsMax,
							debugLevel);         // int         debugLevel)
					imp_mbc.show();
					ImagePlus imp_mbc_merged = QuadCLT.renderGPUFromDSI(
							-1,                  // final int         sensor_mask,
							true,               // final boolean     merge_channels,
							null,                // final Rectangle   full_woi_in,      // show larger than sensor WOI (or null)
							clt_parameters,      // CLTParameters     clt_parameters,
							mb_ref_disparity,    // double []         disparity_ref,
							// motion blur compensation 
							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
							motion_blur,         // double [][]       mb_vectors,  //
							
							mb_scene_xyz,        // ZERO3,               // final double []   scene_xyz, // camera center in world coordinates
							mb_scene_atr,        // final double []   scene_atr, // camera orientation relative to world frame
							quadCLTs[nscene],    // final QuadCLT     scene,
							quadCLTs[ref_index], // final QuadCLT     ref_scene, // now - may be null - for testing if scene is rotated ref
							false, // toRGB,               // final boolean     toRGB,
							clt_parameters.imp.show_color_nan,
							quadCLTs[nscene].getImageName()+"-MOTION_BLUR_CORRECTED-MERGED", // String            suffix,
							THREADS_MAX,          // int               threadsMax,
							debugLevel);         // int         debugLevel)
					imp_mbc_merged.show();
				}
			}
			if (nscene == ref_index) { // set GPU reference CLT data - should be before other scenes
				mb_vectors_ref = null;
				if (mb_en) {
					// should get velocities from HashMap at reference scene from timestamp , not re-calculate.
//					double [][] dxyzatr_dt_ref = getVelocities( // was already set
//							quadCLTs, // QuadCLT []     quadCLTs,
//							ref_index);  // int            nscene)
					double [][] dxyzatr_dt_ref = dxyzatr_dt[nscene];
					mb_vectors_ref = OpticalFlow.getMotionBlur(
							quadCLTs[ref_index],   // QuadCLT        ref_scene,
							quadCLTs[ref_index],   // QuadCLT        scene,         // can be the same as ref_scene
							ref_pXpYD,             // double [][]    ref_pXpYD,     // here it is scene, not reference!
							ZERO3,                 // double []      camera_xyz,
							ZERO3,                 // double []      camera_atr,
							dxyzatr_dt_ref[0],     // double []      camera_xyz_dt,
							dxyzatr_dt_ref[1],     // double []      camera_atr_dt,
							0,                     // int            shrink_gaps,  // will gaps, but not more that grow by this
							debugLevel);           // int            debug_level)
				}
				tp_tasks_ref = Interscene.setReferenceGPU (
						clt_parameters,      // CLTParameters      clt_parameters,			
						quadCLTs[ref_index], // QuadCLT            ref_scene,
						mb_ref_disparity,    // double []          ref_disparity, // null or alternative reference disparity
						ref_pXpYD,           // double [][]        ref_pXpYD,
						reliable_ref,        // final boolean []   selection, // may be null, if not null do not  process unselected tiles
						margin,              // final int          margin,
						// motion blur compensation 
						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
						mb_vectors_ref,      // double [][]        mb_vectors,  // now [2][ntiles];
						debugLevel);         // int         debugLevel)
			} else {
				double [][]        mb_vectors = null;
				scene_xyz_pre = ers_reference.getSceneXYZ(ts);
				scene_atr_pre = ers_reference.getSceneATR(ts);
				double [][] scene_xyzatr = new double[][] {scene_xyz_pre, scene_atr_pre};
				double [][] inv_scene = ErsCorrection.invertXYZATR(scene_xyzatr);
				double [] avg_weights = new double [2*avg_len+1];
				int n_pre =  nscene + avg_len;
				int n_post = nscene - avg_len;
				if (n_post < earliest_scene) n_post = earliest_scene;
				if (n_pre > ref_index) n_pre = ref_index;
				double s0=0, sx=0, sx2=0;
				double [][] sy = new double[2][3], sxy = new double [2][3];
				for (int n_other = n_post; n_other <= n_pre; n_other++) {
					int ix = n_other - nscene; //+/-i
					int i = ix + avg_len; // >=0
					avg_weights[i] = Math.cos(Math.PI * (i - avg_len) / (2 * avg_len + 1));
					double [][] other_xyzatr = ers_reference.getSceneXYZATR(quadCLTs[n_other].getImageName()); 
					double [][] other_rel = (n_other != ref_index)?ErsCorrection.combineXYZATR(other_xyzatr,inv_scene):
						(new double[2][3]);
					double w = avg_weights[i];
					s0 += w;
					sx += w * ix;
					sx2 += w * ix * ix;
					for (int j = 0; j < other_rel.length; j++) {
						for (int k = 0; k < other_rel[j].length; k++) {
							sy [j][k] += w * other_rel[j][k];
							sxy[j][k] += w * other_rel[j][k] * ix;
						}
					}
				}
				boolean        fpn_disable = false; // estimate they are too close
		    	double angle_per_pixel = ers_reference.getCorrVector().getTiltAzPerPixel();
				//ers_reference
				double [][] diff_pull = new double [2][3]; 
				for (int j = 0; j < diff_pull.length; j++) {
					for (int k = 0; k < diff_pull[j].length; k++) {
						diff_pull[j][k]=(sy[j][k]*sx2 - sxy[j][k]*sx) / (s0*sx2 - sx*sx);
					}
				}
				double [][] scene_pull = ErsCorrection.combineXYZATR(scene_xyzatr,diff_pull);
//				double []      min_max = new double[3]; // {min, max, actual rms)
//				int []         fail_reason = new int[1];   // null or int[1]: 0 - OK, 2 - LMA, 3 - min, 4 - max
				
				
				if (mb_en) {
//					double [][] dxyzatr_dt_scene = getVelocities(
//							quadCLTs, // QuadCLT []     quadCLTs,
//							nscene);  // int            nscene)
					double [][] dxyzatr_dt_scene = dxyzatr_dt[nscene];
					mb_vectors = OpticalFlow.getMotionBlur(
							quadCLTs[ref_index],   // QuadCLT        ref_scene,
							quadCLTs[nscene],      // QuadCLT        scene,         // can be the same as ref_scene
							ref_pXpYD,             // double [][]    ref_pXpYD,     // here it is scene, not reference!
							scene_xyz_pre,         // double []      camera_xyz,
							scene_atr_pre,         // double []      camera_atr,
							dxyzatr_dt_scene[0],   // double []      camera_xyz_dt,
							dxyzatr_dt_scene[1],   // double []      camera_atr_dt,
							0,                     // int            shrink_gaps,  // will gaps, but not more that grow by this
							debugLevel);           // int            debug_level)
				}
				double []      lma_rms = new double[2];
				scenes_xyzatr[nscene] = Interscene.adjustPairsLMAInterscene(
						clt_parameters,          // CLTParameters  clt_parameters,
						false,                   // boolean        initial_adjust,
						fpn_disable,             // 			boolean        fpn_disable,   // disable fpn filter if images are known to be too close
						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[ref_index],                            // QuadCLT reference_QuadCLT,
						interscene_ref_disparity,                       // double []        ref_disparity, // null or alternative reference disparity
						ref_pXpYD,                                      // double [][]    pXpYD_ref,     // pXpYD for the reference scene
						reliable_ref,  // boolean []     reliable_ref, // null or bitmask of reliable reference tiles
						tp_tasks_ref[0], // TpTask[]       tp_tasks_ref,  // only (main if MB correction) tasks for FPN correction
						quadCLTs[nscene],                               // QuadCLT scene_QuadCLT,
						scene_xyz_pre,                                  // xyz
						scene_atr_pre,                                  // atr
						scene_pull[0],    // double []      scene_xyz_pull,  // if both are not null, specify target values to pull to
						scene_pull[1],    // double []      scene_atr_pull, 
						clt_parameters.ilp.ilma_lma_select,             // final boolean[]   param_select,
						clt_parameters.ilp.ilma_regularization_weights, //  final double []   param_regweights,
						lma_rms,                                        // double []      rms, // null or double [2]
						clt_parameters.imp.max_rms,                     // double         max_rms,
						// motion blur compensation 
						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
						mb_vectors,  // double [][]    mb_vectors,  // now [2][ntiles];
						clt_parameters.imp.debug_level);                // 1); // -1); // int debug_level);
				boolean adjust_OK = scenes_xyzatr[nscene] != null;
//				if (adjust_OK && (Math.abs(scenes_xyzatr[nscene][1][2]) > max_roll)) {
//					fail_reason[0] = FAIL_REASON_ROLL;
//					adjust_OK = false;
//				}
				if (adjust_OK && fail_on_zoom_roll) { // check only for initial orientation, do not check on readjustments
					if (Math.abs(scenes_xyzatr[nscene][1][2]) > max_roll) {
						fail_reason[0] = FAIL_REASON_ROLL;
						adjust_OK = false;
					}
					if (max_zoom_diff > 0) { // ignore if set to 
						if (Math.abs(scenes_xyzatr[nscene][0][2]) > max_z_change) {  // NaN OK - will not fail
							fail_reason[0] = FAIL_REASON_ZOOM;
							adjust_OK = false;
						}
					}
				}
				
				// FAIL_REASON_ROLL
				handle_failure: {
					if (!adjust_OK) {
						boolean OK = false;
						System.out.println("LMA failed at nscene = "+nscene+". Reason = "+fail_reason[0]+
								" ("+getFailReason(fail_reason[0])+")");
						if ((fail_reason[0]==FAIL_REASON_MIN) || ((fail_reason[0]==FAIL_REASON_LMA))) {
							if (fpn_skip) {
								System.out.println("fpn_skip is set, just skipping this scene");
								failed_scenes[nscene] = true;
								num_failed++;
								scenes_xyzatr[nscene] = new double[][] {
									scene_xyz_pre, // for now restore original, pre-adjustment
									scene_atr_pre  // for now restore original, pre-adjustment
								};
								break handle_failure;
//								continue; // to next scene
							} else {
								System.out.println("fpn_skip is not set, aborting series and adjusting earliest_scene");
								// set this and all previous to null
							}
						}
						// all other reasons lead to failure
						earliest_scene = nscene + 1;
						if (debugLevel > -4) {
							System.out.println("reAdjustPairsLMAInterscene "+nscene+" (of "+ quadCLTs.length+") "+
									quadCLTs[ref_index].getImageName() + "/" + ts+
									" FAILED. Setting earliest_scene to "+earliest_scene + " Failure reason: " + fail_reason[0]+
									" ("+getFailReason(fail_reason[0])+")");
						}
						// set this and all previous to null
						for (; nscene >= 0 ; nscene--) {
							ers_reference.addScene(quadCLTs[nscene].getImageName(), null);
							quadCLTs[nscene] = null; // completely remove early scenes?
						}
						break;
					}
				}
				// overwrite old data
				ers_reference.addScene(ts,
						scenes_xyzatr[nscene][0],
						scenes_xyzatr[nscene][1],
						quadCLTs[nscene].getErsCorrection().getErsXYZ_dt(),	// same as dxyzatr_dt[nscene][0], just keep for future adjustments?	
						quadCLTs[nscene].getErsCorrection().getErsATR_dt()	// same as dxyzatr_dt[nscene][1], just keep for future adjustments?		
						);
				if (lma_rms[0] > maximal_series_rms) {
					maximal_series_rms = lma_rms[0];
				}

				if (debugLevel > -3) {
					System.out.println("reAdjustPairsLMAInterscene "+nscene+" (of "+ quadCLTs.length+") "+
							quadCLTs[ref_index].getImageName() + "/" + ts+
							" Done. RMS="+lma_rms[0]+", maximal so far was "+maximal_series_rms);
				}
			}
        } // for (int nscene = ref_index; nscene > earliest_scene; nscene--) {

        // TODO: after all scenes done, see if any of scenes_xyzatr[] is null, and if fpn_rematch - rematch
        
		if (dbg_mb_img != null) {
			String [] dbg_mb_titles = new String[quadCLTs.length];
			for (int i = 0; i < quadCLTs.length; i++) if (quadCLTs[i] != null) {
				dbg_mb_titles[i] = quadCLTs[i].getImageName();
			}
			ShowDoubleFloatArrays.showArrays(
					dbg_mb_img,
					tilesX * 2,
					tilesY,
					true,
					quadCLTs[ref_index].getImageName()+"-MOTION_BLUR",
					dbg_mb_titles);
		}
        
		if (debugLevel > -4) {
			System.out.println("All multi scene passes are Done. Maximal RMSE was "+maximal_series_rms);
		}

		quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
				null, // String path,             // full name with extension or w/o path to use x3d directory
				debugLevel+1);
		return earliest_scene;
	}
	
	public static double[][]  adjustPairsLMAInterscene(
			CLTParameters  clt_parameters,
		    boolean        use_lma_dsi,
			boolean        fpn_disable,   // disable fpn filter if images are known to be too close
			double []      min_max,       // null or pair of minimal and maximal offsets
			int []         fail_reason,   // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
			QuadCLT        reference_QuadClt,
			double []      ref_disparity, // null or alternative reference disparity
			boolean []     reliable_ref, // null or bitmask of reliable reference tiles
			QuadCLT        scene_QuadClt,
			double []      camera_xyz,
			double []      camera_atr,
			double []      scene_xyz_pull, // if both are not null, specify target values to pull to 
			double []      scene_atr_pull, // 
			boolean[]      param_select,
			double []      param_regweights,
			double []      rms_out, // null or double [2]
			double         max_rms,
			int            debug_level)
	{
//	    boolean use_lma_dsi =          clt_parameters.imp.use_lma_dsi;
		if (ref_disparity == null) { 
			ref_disparity = reference_QuadClt.getDLS()[use_lma_dsi?1:0];
		}

		// TODO: set reference as new version of adjustPairsLMAInterscene() assumes it set
		int        margin = clt_parameters.imp.margin;
		double [][] pXpYD_ref = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
				null,               // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
				ref_disparity,      // dls[0],  // final double []   disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
				ZERO3,              // final double []   scene_xyz, // camera center in world coordinates
				ZERO3,              // final double []   scene_atr, // camera orientation relative to world frame
				reference_QuadClt,  // final QuadCLT     scene_QuadClt,
				reference_QuadClt); // final QuadCLT     reference_QuadClt)
		TpTask[][] tp_tasks_ref2 = setReferenceGPU (
				clt_parameters,     // CLTParameters      clt_parameters,			
				reference_QuadClt,  // QuadCLT            ref_scene,
				ref_disparity,      // double []          ref_disparity, // null or alternative reference disparity
				pXpYD_ref,          // double [][]        ref_pXpYD,
				reliable_ref,       // final boolean []   selection, // may be null, if not null do not  process unselected tiles
				margin,             // final int          margin,
				// motion blur compensation 
				0.0,                // double             mb_tau,      // 0.008; // time constant, sec
				0.0,                // double             mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
				null,               // double [][]        mb_vectors,  // now [2][ntiles];
				debug_level);       // int                debug_level)
		return  adjustPairsLMAInterscene( // assumes reference GPU data is already set - once for multiple scenes
				clt_parameters,    // CLTParameters  clt_parameters,
				true,              // boolean        initial_adjust,
				false,             // boolean        fpn_disable,   // disable fpn filter if images are known to be too close
				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
				reference_QuadClt, // QuadCLT        reference_QuadClt,
				ref_disparity,     // double []      ref_disparity, // null or alternative reference disparity
				pXpYD_ref,         //				double [][]    pXpYD_ref,     // pXpYD for the reference scene
				reliable_ref,      // boolean []     reliable_ref, // null or bitmask of reliable reference tiles
				tp_tasks_ref2[0], // TpTask[]       tp_tasks_ref,  // only (main if MB correction) tasks for FPN correction

				scene_QuadClt,     // QuadCLT        scene_QuadClt,
				camera_xyz,        // double []      camera_xyz,
				camera_atr,        // double []      camera_atr,
				scene_xyz_pull,    // double []      scene_xyz_pull,  // if both are not null, specify target values to pull to
				scene_atr_pull,    // double []      scene_atr_pull, 
				param_select,      // boolean[]      param_select,
				param_regweights,  // double []      param_regweights,
				rms_out,           // double []      rms_out, // null or double [2]
				max_rms,           // double         max_rms,
				// motion blur compensation 
				0.0,               // double         mb_tau,      // 0.008; // time constant, sec
				0.0,               // double         mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
				null,              // double [][]    mb_vectors,  // now [2][ntiles];
				debug_level);      // int            debug_level)
	}

	public static double[][]  adjustPairsLMAInterscene( // assumes reference scene already set in GPU.
			CLTParameters  clt_parameters,
			boolean        initial_adjust,
			boolean        fpn_disable,   // disable fpn filter if images are known to be too close
			double []      min_max,       // null or pair of minimal and maximal offsets
			int []         fail_reason,   // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
			QuadCLT        reference_QuadClt,
			double []      ref_disparity, // null or alternative reference disparity
			double [][]    pXpYD_ref,     // pXpYD for the reference scene
			boolean []     reliable_ref, // null or bitmask of reliable reference tiles
			TpTask[]       tp_tasks_ref,  // only (main if MB correction) tasks for FPN correction
			QuadCLT        scene_QuadClt,
			double []      camera_xyz,
			double []      camera_atr,
			double []      scene_xyz_pull, // if both are not null, specify target values to pull to 
			double []      scene_atr_pull, // 
			boolean[]      param_select,
			double []      param_regweights,
			double []      rms_out, // null or double [2]
			double         max_rms,
			// motion blur compensation 
			double         mb_tau,      // 0.008; // time constant, sec
			double         mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
			double [][]    mb_vectors,  // now [2][ntiles];
			int            debug_level)
	{
		boolean use3D = clt_parameters.ilp.ilma_3d;
		boolean filter_by_ers =   clt_parameters.ilp.ilma_ers_quads;
		double  gap_frac =        clt_parameters.ilp.ilma_gap_frac ; // 0.25;
		int     min_quad_tiles =  clt_parameters.ilp.ilma_min_quad_tiles ; // 10;
		double  min_quad_weight = clt_parameters.ilp.ilma_min_quad_weight ; // 0.01;
		
//		boolean use3D_lma = clt_parameters.ilp.ilma_3d_lma;
		double disparity_weight = use3D? clt_parameters.ilp.ilma_disparity_weight : 0.0;
		int        margin = clt_parameters.imp.margin;
		int        sensor_mask_inter = clt_parameters.imp.sensor_mask_inter ; //-1;
		float [][][] facc_2d_img = new float [1][][]; // set it to null?
		IntersceneLma intersceneLma = new IntersceneLma(
				clt_parameters.ilp.ilma_thread_invariant,
				disparity_weight);
		int lmaResult = -1;
		boolean last_run = false;
		double[] camera_xyz0 = camera_xyz.clone();
		double[] camera_atr0 = camera_atr.clone();
		double [][][] coord_motion = null;
		boolean show_corr_fpn = (!clt_parameters.multiseq_run) && (debug_level > -1); //  -3; *********** Change to debug FPN correleation *** 
		
		int nlma = 0;
		for (; nlma < clt_parameters.imp.max_cycles; nlma ++) {
			boolean near_important = nlma > 0;
			// FIXME: not re-calculating motion blur. Maybe do that after each parameter update?
			// pass dxyzatr_dt, not mb_vectors? Or update velocities too?
			coord_motion = interCorrPair( // new double [tilesY][tilesX][][];
					clt_parameters,      // CLTParameters  clt_parameters,
					use3D,               // boolean        use3D,         // generate disparity difference
					fpn_disable,         //	boolean        fpn_disable,   // disable fpn filter if images are known to be too close
					mb_max_gain,         // double         mb_max_gain,					
					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
					reference_QuadClt,   // QuadCLT        reference_QuadCLT,
					ref_disparity,       // double []      ref_disparity, // null or alternative reference disparity
					pXpYD_ref,           // double [][]    pXpYD_ref,     // pXpYD for the reference scene			
					tp_tasks_ref,        // TpTask[]       tp_tasks_ref,  // only (main if MB correction) tasks for FPN correction
					scene_QuadClt,       // QuadCLT        scene_QuadCLT,
					camera_xyz0,         //                xyz
					camera_atr0,         // pose[1],      // atr
					reliable_ref,        // ****null,     // final boolean [] selection, // may be null, if not null do not  process unselected tiles
					margin,              // final int      margin,
					sensor_mask_inter,   // final int      sensor_mask_inter, // The bitmask - which sensors to correlate, -1 - all.
					facc_2d_img,         // final float [][][]   accum_2d_corr, // if [1][][] - return accumulated 2d correlations (all pairs)final float [][][]   accum_2d_corr, // if [1][][] - return accumulated 2d correlations (all pairs)
					null,                //	final float [][] dbg_corr_fpn,
					near_important,      // boolean            near_important, // do not reduce weight of the near tiles
					false,               // boolean            all_fpn,        // do not lower thresholds for non-fpn (used during search)
					initial_adjust,      // boolean             initial_adjust,
					mb_vectors,          // double [][]        mb_vectors,  // now [2][ntiles];
					clt_parameters.imp.debug_level, // int                imp_debug_level,
					debug_level); // 1); // -1); // int debug_level);
	        if (coord_motion == null) {
	        	System.out.println("adjustPairsLMAInterscene() returned null");
	        	return null;
	        }
	        boolean ers_lma = false;
	        for (int i:ErsCorrection.DP_ERS_INDICES) {
	        	ers_lma |= param_select[i];
	        }
			boolean disable_ers=false;
			boolean [] param_select_mod = param_select; 
			if (ers_lma && filter_by_ers) {
				double [][] quad_strengths = getQuadStrengths(
						coord_motion,                                      // double [][][] coord_motion,
						gap_frac,                                          //double           gap_frac, // 0.25 
						reference_QuadClt.getTileProcessor().getTilesX()); // int           tilesX);
				for (int i = 0; i < quad_strengths[0].length; i++) {
					if ((quad_strengths[0][i] < min_quad_tiles) || (quad_strengths[1][i] < min_quad_weight)) {
						disable_ers = true;
						break;
					}
				}
				if (disable_ers) {
					param_select_mod = param_select.clone();
					for (int i:ErsCorrection.DP_ERS_INDICES) {
						param_select_mod[i] = false;
					}
					// now just copy ers from the reference 
					ErsCorrection ers_ref =   reference_QuadClt.getErsCorrection();
					ErsCorrection ers_scene = scene_QuadClt.getErsCorrection();
					ers_scene.ers_watr_center_dt = ers_ref.ers_watr_center_dt.clone();
					ers_scene.ers_wxyz_center_dt = ers_ref.ers_wxyz_center_dt.clone();
					if (clt_parameters.imp.debug_level > -2) {
						System.out.print("adjustPairsLMAInterscene(): insufficient data for ERS, skipping. ");
						System.out.println ("quad_defined = ["+((int)quad_strengths[0][0])+","
								+((int)quad_strengths[0][1])+","+((int)quad_strengths[0][2])+","+((int)quad_strengths[0][3])+"] (needed = "+
								min_quad_tiles +"), rel_strengths = ["
								+quad_strengths[1][0]+","+quad_strengths[1][1]+","
								+quad_strengths[1][2]+","+quad_strengths[1][3]+"] (needed "
								+min_quad_weight+")");
					}
				}
			}
	        
			intersceneLma.prepareLMA(
					camera_xyz0,         // final double []   scene_xyz0,     // camera center in world coordinates (or null to use instance)
					camera_atr0,         // final double []   scene_atr0,     // camera orientation relative to world frame (or null to use instance)
					scene_xyz_pull,                                           // final double []   scene_xyz_pull, // if both are not null, specify target values to pull to 
					scene_atr_pull,                                           // final double []   scene_atr_pull, // 
					// reference atr, xyz are considered 0.0
					scene_QuadClt,       // final QuadCLT     scene_QuadClt,
					reference_QuadClt,   // final QuadCLT     reference_QuadClt,
					param_select_mod, // param_select,        // final boolean[]   param_select,
					param_regweights,    // final double []   param_regweights,
					coord_motion[1],     // final double [][] vector_XYS, // optical flow X,Y, confidence obtained from the correlate2DIterate()
					coord_motion[0],     // final double [][] centers,    // macrotile centers (in pixels and average disparities
					(nlma == 0),         // boolean           first_run,
					clt_parameters.imp.debug_level);           // final int         debug_level)
			lmaResult = intersceneLma.runLma(
					clt_parameters.ilp.ilma_lambda, // double lambda,           // 0.1
					clt_parameters.ilp.ilma_lambda_scale_good, //  double lambda_scale_good,// 0.5
					clt_parameters.ilp.ilma_lambda_scale_bad,  // double lambda_scale_bad, // 8.0
					clt_parameters.ilp.ilma_lambda_max,        // double lambda_max,       // 100
					clt_parameters.ilp.ilma_rms_diff,          // double rms_diff,         // 0.001
					clt_parameters.imp.max_LMA,                // int    num_iter,         // 20
					last_run,                                  // boolean last_run,
					debug_level);           // int    debug_level)
			if (lmaResult < 0) {
				System.out.println("Interscene adjustment failed, lmaResult="+lmaResult+" < 0");
				if (fail_reason != null) {
					fail_reason[0]=FAIL_REASON_LMA;
				}
				return null;
			}
			camera_xyz0 = intersceneLma.getSceneXYZ(false); // true for initial values
			camera_atr0 = intersceneLma.getSceneATR(false); // true for initial values
			double [] diffs_atr = intersceneLma.getV3Diff(ErsCorrection.DP_DSAZ);
			double [] diffs_xyz = intersceneLma.getV3Diff(ErsCorrection.DP_DSX);
			if ((diffs_atr[0] < clt_parameters.imp.exit_change_atr) &&
					(diffs_xyz[0] < clt_parameters.imp.exit_change_xyz)) {
				break;
			}
		}
		if (show_corr_fpn && (debug_level > -1)) { // now not needed, restore if needed
//			int num_components = intersceneLma.getNumComponents();
			int num_components = 2;
			String [] fpn_dbg_titles = new String[num_components + scene_QuadClt.getNumSensors() * 2];
			fpn_dbg_titles[0] = "X_avg";
			fpn_dbg_titles[1] = "Y_avg";
			if (num_components > 2) {
				fpn_dbg_titles[2] = "Disp";
			}
			for (int i = 0; i < scene_QuadClt.getNumSensors(); i++) {
				fpn_dbg_titles[num_components + 0 + 2*i] = "X-"+i;
				fpn_dbg_titles[num_components + 1 + 2*i] = "Y-"+i;
			}
			float [][] dbg_corr_fpn = new float [fpn_dbg_titles.length][];
			coord_motion = interCorrPair( // new double [tilesY][tilesX][][];
					clt_parameters,      // CLTParameters  clt_parameters,
					false,               // boolean        use3D,         // generate disparity difference
					false, // 			boolean            fpn_disable,   // disable fpn filter if images are known to be too close
					mb_max_gain,         // double         mb_max_gain,	
					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
					reference_QuadClt,   // QuadCLT reference_QuadCLT,
					ref_disparity,       // double []        ref_disparity, // null or alternative reference disparity
					pXpYD_ref,           // double [][]        pXpYD_ref,     // pXpYD for the reference scene			
					tp_tasks_ref,        // TpTask[]           tp_tasks_ref,  // only (main if MB correction) tasks for FPN correction
					scene_QuadClt,       // QuadCLT scene_QuadCLT,
					camera_xyz0,         // xyz
					camera_atr0,         // pose[1], // atr
					reliable_ref,        // ****null,                // final boolean [] selection, // may be null, if not null do not  process unselected tiles
					margin,              // final int        margin,
					sensor_mask_inter,   // final int        sensor_mask_inter, // The bitmask - which sensors to correlate, -1 - all.
					facc_2d_img,         // final float [][][]   accum_2d_corr, // if [1][][] - return accumulated 2d correlations (all pairs)final float [][][]   accum_2d_corr, // if [1][][] - return accumulated 2d correlations (all pairs)
					dbg_corr_fpn,        //	final float [][] dbg_corr_fpn,
					true,                // boolean            near_important, // do not reduce weight of the near tiles
					false,               // boolean            all_fpn,        // do not lower thresholds for non-fpn (used during search)
					initial_adjust,      // boolean            initial_adjust,					
					mb_vectors,          // double [][]        mb_vectors,  // now [2][ntiles];
					2,                   // int                imp_debug_level,
					debug_level); // 1); // -1); // int debug_level);
			TileProcessor tp = reference_QuadClt.getTileProcessor();
			int tilesX = tp.getTilesX();
			int tilesY = tp.getTilesY();

			ShowDoubleFloatArrays.showArrays(
					dbg_corr_fpn,
					tilesX,
					tilesY,
					true,
					scene_QuadClt.getImageName()+"-"+reference_QuadClt.getImageName()+"-CORR-FPN",
					fpn_dbg_titles);
		}
		if (show_corr_fpn) { // repeat after last adjustment to get images
//			int num_components = intersceneLma.getNumComponents();
			int num_components = 2; // herte - no disparity
			String [] fpn_dbg_titles = new String[num_components + scene_QuadClt.getNumSensors() * 2];
			fpn_dbg_titles[0] = "X_avg";
			fpn_dbg_titles[1] = "Y_avg";
			if (num_components > 2) {
				fpn_dbg_titles[2] = "Disp";
			}
			for (int i = 0; i < scene_QuadClt.getNumSensors(); i++) {
				fpn_dbg_titles[num_components + 0 + 2*i] = "X-"+i;
				fpn_dbg_titles[num_components + 1 + 2*i] = "Y-"+i;
			}
			float [][] dbg_corr_fpn = new float [fpn_dbg_titles.length][];
			coord_motion = interCorrPair( // new double [tilesY][tilesX][][];
					clt_parameters,      // CLTParameters  clt_parameters,
					false,               // boolean        use3D,         // generate disparity difference
					false, // 			boolean            fpn_disable,   // disable fpn filter if images are known to be too close
					mb_max_gain,         // double         mb_max_gain,
					null,                // double []      min_max,       // null or pair of minimal and maximal offsets
					null,                // int []         fail_reason,   // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
					reference_QuadClt,   // QuadCLT reference_QuadCLT,
					ref_disparity,       // double []        ref_disparity, // null or alternative reference disparity
					pXpYD_ref,           // double [][]        pXpYD_ref,     // pXpYD for the reference scene			
					tp_tasks_ref,        // TpTask[]           tp_tasks_ref,  // only (main if MB correction) tasks for FPN correction
					scene_QuadClt,       // QuadCLT scene_QuadCLT,
					camera_xyz0,         // xyz
					camera_atr0,         // pose[1], // atr
					reliable_ref,        // ****null,                // final boolean [] selection, // may be null, if not null do not  process unselected tiles
					margin,              // final int        margin,
					sensor_mask_inter,   // final int        sensor_mask_inter, // The bitmask - which sensors to correlate, -1 - all.
					facc_2d_img,         // final float [][][]   accum_2d_corr, // if [1][][] - return accumulated 2d correlations (all pairs)final float [][][]   accum_2d_corr, // if [1][][] - return accumulated 2d correlations (all pairs)
					dbg_corr_fpn,        //	final float [][] dbg_corr_fpn,
					true,                // boolean            near_important, // do not reduce weight of the near tiles
					false,               // boolean            all_fpn,        // do not lower thresholds for non-fpn (used during search)
					initial_adjust,      // boolean            initial_adjust,					
					mb_vectors,          // double [][]        mb_vectors,  // now [2][ntiles];
					2,                   // int                imp_debug_level,
					debug_level); // 1); // -1); // int debug_level);
			TileProcessor tp = reference_QuadClt.getTileProcessor();
			int tilesX = tp.getTilesX();
			int tilesY = tp.getTilesY();

			ShowDoubleFloatArrays.showArrays(
					dbg_corr_fpn,
					tilesX,
					tilesY,
					true,
					scene_QuadClt.getImageName()+"-"+reference_QuadClt.getImageName()+"-CORR-FPN",
					fpn_dbg_titles);
		}
		
		
		
		if (clt_parameters.imp.debug_level > -2) {
			String [] lines1 = intersceneLma.printOldNew((clt_parameters.imp.debug_level > -1)); // false); // boolean allvectors)
			System.out.println("Adjusted interscene, iteration="+nlma+
					", last RMS = "+intersceneLma.getLastRms()[0]+
					" (pure RMS = "+intersceneLma.getLastRms()[1]+")"+
					", results:");
			for (String line : lines1) {
				System.out.println(line);
			}
		}
		if ((rms_out != null) && (intersceneLma.getLastRms() != null)) {
			rms_out[0] = intersceneLma.getLastRms()[0];
			rms_out[1] = intersceneLma.getLastRms()[1];
			//if (lmaResult < 0) { last_rms[0]
		}
		if (max_rms > 0.0) {
			if (lmaResult < 0) { // = 0) {
				return null;
			}
			if (!(intersceneLma.getLastRms()[0] <= max_rms)) {
				System.out.println("RMS failed: "+intersceneLma.getLastRms()[0]+" >= " + max_rms);
				return null;
			}
		}
		return new double [][] {camera_xyz0, camera_atr0};
	}	
	
	
	
	
	
    public static double [] spiralSearchATR(
    		CLTParameters             clt_parameters,
        	boolean                   use_lma_dsi, // may be turned off by the caller if there are too few points
    		int                       search_rad, // to reduce for the next scenes on failure (FPN filter)
    		QuadCLT                   reference_QuadClt,
    		QuadCLT                   scene_QuadClt,
    		boolean []                reliable_ref,
    		int                       debugLevel
    		) {
//		boolean use3D = clt_parameters.ilp.ilma_3d && (clt_parameters.ilp.ilma_disparity_weight > 0);
//		double disparity_weight = use3D? clt_parameters.ilp.ilma_disparity_weight : 0.0;
    	int [][] offset_start_corner = {{-1,-1},{1,-1},{1,1},{-1,1}}; //{x,y}
    	int [][] offset_move = {{1,0},{0,1},{-1,0},{0,-1}};
    	int    margin =            clt_parameters.imp.margin;
    	int    sensor_mask_inter = clt_parameters.imp.sensor_mask_inter ; //-1;
    	float [][][] facc_2d_img = new float [1][][];
    	int    pix_step =          clt_parameters.imp.pix_step;      // 4;
    	// int    search_rad =        clt_parameters.imp.search_rad;    // 10;
    	double maybe_sum =         clt_parameters.imp.maybe_sum;     // 8.0;
    	double sure_sum =          clt_parameters.imp.sure_sum;     // 30.0;
    	double maybe_avg =         clt_parameters.imp.maybe_avg;     // 0.005;
    	double sure_avg =          clt_parameters.imp.sure_avg;     // 0.015;
    	
    	double  max_search_rms =   clt_parameters.imp.max_search_rms; //             1.2;   // good - 0.34, so-so - 0.999
    	double  maybe_fom =        clt_parameters.imp.maybe_fom;      //             3.0;   // good - 30, second good - 5
    	double  sure_fom =         clt_parameters.imp.sure_fom;       //            10.0;   // good - 30, second good - 10
    	boolean treat_serch_fpn =  clt_parameters.imp.treat_serch_fpn;// use FPN (higher) thresholds during search (even if offset is not small)

//    	boolean use_lma_dsi =      clt_parameters.imp.use_lma_dsi;
    	
		double [][] pose = new double [2][3];
    	double angle_per_step = reference_QuadClt.getGeometryCorrection().getCorrVector().getTiltAzPerPixel() * pix_step;
    	double [][] atrs = new double [(2*search_rad+1)*(2*search_rad+1)][];
    	boolean [] good_offset =    new boolean [atrs.length];
    	double [] confidences_sum = new double [atrs.length];
    	double [] confidences_avg = new double [atrs.length];
    	double [] confidences_fom = new double [atrs.length];
    	int min_defined =           75;
    	int rad = 0, dir=0, n=0;
    	int ntry = 0;
    	int num_good=0;
    	double [] use_atr = null;
    	int dbg_ntry = 61;
    	
    	double []ref_disparity = reference_QuadClt.getDLS()[use_lma_dsi?1:0];
    	double [][]  pXpYD_ref = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
    			null,               // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
    			ref_disparity,      // dls[0],  // final double []   disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
    			ZERO3,              // final double []   scene_xyz, // camera center in world coordinates
    			ZERO3,              // final double []   scene_atr, // camera orientation relative to world frame
    			reference_QuadClt,  // final QuadCLT     scene_QuadClt,
    			reference_QuadClt); // final QuadCLT     reference_QuadClt)
    	TpTask[] tp_tasks_ref = setReferenceGPU (
    			clt_parameters,     // CLTParameters      clt_parameters,			
    			reference_QuadClt,  // QuadCLT            ref_scene,
    			ref_disparity,      // double []          ref_disparity, // null or alternative reference disparity
    			pXpYD_ref,          // double [][]        ref_pXpYD,
    			reliable_ref,       // final boolean []   selection, // may be null, if not null do not  process unselected tiles
    			margin,             // final int          margin,
    			// motion blur compensation 
    			0.0,                // double             mb_tau,      // 0.008; // time constant, sec
    			0.0,                // double             mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
    			null,               // double [][]        mb_vectors,  // now [2][ntiles];
    			debugLevel)[0];       // int                debug_level)
    	
    	try_around:
    		for (rad = 00; rad <= search_rad; rad++) {
    			for (dir = 0; dir < ((rad==0)?1:4); dir++) {
    				int n_range = (rad > 0) ? (2* rad) : 1;
    				for (n = 0; n < n_range; n++) {
    					int ix = rad*offset_start_corner[dir][0] + n * offset_move[dir][0];
    					int iy = rad*offset_start_corner[dir][1] + n * offset_move[dir][1];;
    					double [] atr = {
    							pose[1][0]+ix * angle_per_step,
    							pose[1][1]+iy * angle_per_step,
    							pose[1][2]};
    					if (debugLevel > -1) {
    						System.out.println("buildSeries(): trying adjustPairsLMA() with initial offset azimuth: "+
    								atr[0]+", tilt ="+atr[1]);
    					}
    					if (ntry==dbg_ntry) {
    						System.out.println("ntry="+ntry);
    					}
    					double [][][] coord_motion = interCorrPair( // new double [tilesY][tilesX][][];
    							clt_parameters,      // CLTParameters  clt_parameters,
    							false, // use3D,               // boolean        use3D,         // generate disparity difference
    							false, // 			boolean            fpn_disable,   // disable fpn filter if images are known to be too close
    							0,                   // mb_max_gain,         // double  mb_max_gain,
    							null,                // double []      min_max,       // null or pair of minimal and maximal offsets
    							null,                // int []         fail_reason,   // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
    							reference_QuadClt,   // QuadCLT reference_QuadCLT,
    							ref_disparity,       // null, // double []        ref_disparity, // null or alternative reference disparity
    							pXpYD_ref,   // 			double [][]        pXpYD_ref,     // pXpYD for the reference scene	11653		
    							tp_tasks_ref, // 			TpTask[]           tp_tasks_ref,  // only (main if MB correction) tasks for FPN correction
    							scene_QuadClt,       // QuadCLT scene_QuadCLT,
    							pose[0],             // camera_xyz0,         // xyz
    							atr,                 // camera_atr0,         // pose[1], // atr
    							reliable_ref, // null,                // final boolean [] selection, // may be null, if not null do not  process unselected tiles
    							margin,              // final int        margin,
    							sensor_mask_inter,   // final int        sensor_mask_inter, // The bitmask - which sensors to correlate, -1 - all.
    							facc_2d_img,         // final float [][][]   accum_2d_corr, // if [1][][] - return accumulated 2d correlations (all pairs)final float [][][]   accum_2d_corr, // if [1][][] - return accumulated 2d correlations (all pairs)
    							null,                //	final float [][][] dbg_corr_fpn,
    							false,               // boolean            near_important, // do not reduce weight of the near tiles
    							treat_serch_fpn,     // true,                // boolean            all_fpn,        // do not lower thresholds for non-fpn (used during search)
    							true,                // boolean             initial_adjust,
    							null,                // double [][]        mb_vectors,  // now [2][ntiles];
    							clt_parameters.imp.debug_level, // int                imp_debug_level,
    							clt_parameters.imp.debug_level); // 1); // -1); // int debug_level);
    					// may return null if failed to match
    					if (coord_motion == null) {
    			    		System.out.println("Failed to find a match between the reference scene ("+reference_QuadClt.getImageName() +
    			    				") and a previous one ("+scene_QuadClt.getImageName()+"), coord_motion == null");
    			    		return null;
    					}
    					int num_defined = 0;
    					double sum_strength = 0.0;
    					// calculate average vector and standard deviation for its components
    					double swx=0.0,swy = 0.0, swx2 = 0.0, swy2 = 0.0, sw2=0.0 ; 
    					// coord_motion may be null here
    					for (int i = 0; i < coord_motion[1].length; i++) if (coord_motion[1][i] != null){
    						swx +=  coord_motion[1][i][2] * coord_motion[1][i][0];
    						swx2 += coord_motion[1][i][2] * coord_motion[1][i][0] * coord_motion[1][i][0];
    						swy +=  coord_motion[1][i][2] * coord_motion[1][i][1];
    						swy2 += coord_motion[1][i][2] * coord_motion[1][i][1] * coord_motion[1][i][1];
    						sum_strength += coord_motion[1][i][2];
    						sw2+=coord_motion[1][i][2]*coord_motion[1][i][2];
    						num_defined++;
    					}
    					double vx_avg = swx/sum_strength;
    					double vy_avg = swy/sum_strength;
    					double l2x = swx2/sum_strength - vx_avg * vx_avg;
    					double l2y = swy2/sum_strength - vy_avg * vy_avg;
    					double rms_x = Math.sqrt(l2x);
    					double rms_y= Math.sqrt(l2y);
    					double rms = Math.sqrt(l2x+l2y);
    					double fom = sum_strength/rms/Math.sqrt(0.01 * num_defined); // reduce influence of number defined
    					double avg_strength = (num_defined > 0)?(sum_strength/num_defined) : 00;
    					atrs[ntry] = atr;
    					confidences_sum[ntry] = sum_strength;
    					confidences_avg[ntry] = avg_strength;
    					confidences_fom[ntry] = fom;
    					boolean good = (num_defined > min_defined) && (rms < max_search_rms) && (fom > maybe_fom) && (sum_strength > maybe_sum) && (avg_strength > maybe_avg);
    					good_offset[ntry] = good;
    					if (debugLevel > -2) {
    						System.out.println ((good?"* ":"  ")+"ntry = "+ntry+", num_defined = "+num_defined+" ("+min_defined+")"+
    								", FOM="+fom+
    								", vx_avg = "+vx_avg+", vy_avg="+vy_avg+
    								", sum_strength = "+sum_strength+", rms="+rms+
    								", str2="+Math.sqrt(sw2)+ ", str2_avg="+Math.sqrt(sw2/num_defined)+
    								", avg_strength = "+avg_strength+
    								", rms_x="+rms_x+", rms_y="+rms_y);
    					}
    					if (good) {
    						if ((fom > sure_fom) || (sum_strength > sure_sum) || (avg_strength > sure_avg)) {
    							use_atr = atr;
    							break try_around;
    						}
    						num_good++;
    					}
//    					if (ntry == 105) {
//    						System.out.println("spiralSearchATR(): ntry="+ntry);
//    					}
    					ntry++;
    				}
    			}
    		}
    	if ((use_atr == null) && (num_good == 0)) {
    		System.out.println("Failed to find a match between the reference scene ("+reference_QuadClt.getImageName() +
    				") and a previous one ("+scene_QuadClt.getImageName()+")");
    		return null;
    	}
    	if (use_atr == null) {
    		int best_ntry = -1;
    		for (int i = 0; i <  atrs.length; i++) if (good_offset[i]){
       			if ((best_ntry < 0) || (confidences_fom[i] > confidences_fom[best_ntry])) {
    				best_ntry = i;
    			}
    		}
    		use_atr = atrs[best_ntry];
    	}
    	return use_atr;
    }
	
	
	/**
	 * 
	 * @param clt_parameters
	 * @param use3D           when true, generate disparity difference in addition to average {pX,pY} 
	 * @param mb_max_gain
	 * @param ref_scene
	 * @param ref_disparity
	 * @param pXpYD_ref
	 * @param tp_tasks_ref
	 * @param scene
	 * @param scene_xyz
	 * @param scene_atr
	 * @param selection
	 * @param margin
	 * @param sensor_mask_inter
	 * @param accum_2d_corr
	 * @param dbg_corr_fpn
	 * @param near_important
	 * @param all_fpn
	 * @param mb_vectors
	 * @param imp_debug_level
	 * @param debug_level
	 * @return
	 */
	
	public static double [][][]  interCorrPair( // return [tilesX*telesY]{ref_pXpYD, dXdYS}
			CLTParameters      clt_parameters,
			boolean            use3D,         // generate disparity difference
			boolean            fpn_disable,   // disable fpn filter if images are known to be too close
			double             mb_max_gain,
			double []          min_max,       // null or pair of minimal and maximal offsets
			int []             fail_reason,   // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
			QuadCLT            ref_scene,
			double []          ref_disparity, // null or alternative reference disparity
			double [][]        pXpYD_ref,     // pXpYD for the reference scene			
			TpTask[]           tp_tasks_ref,  // only (main if MB correction) tasks for FPN correction
			QuadCLT            scene,
			double []          scene_xyz,
			double []          scene_atr,
			final boolean []   selection, // may be null, if not null do not  process unselected tiles
			final int          margin,
			final int          sensor_mask_inter, // The bitmask - which sensors to correlate, -1 - all.
			final float [][][] accum_2d_corr, // if [1][][] - return accumulated 2d correlations (all pairs)final float [][][]   accum_2d_corr, // if [1][][] - return accumulated 2d correlations (all pairs)
			final float [][]   dbg_corr_fpn,
			boolean            near_important, // do not reduce weight of the near tiles
			boolean            all_fpn,        // do not lower thresholds for non-fpn (used during search)
			boolean            initial_adjust,
			// motion blur compensation 
			double [][]        mb_vectors,  // now [2][ntiles];
			int                imp_debug_level, // MANUALLY change to 2 for debug!
			int                debug_level)
	{
		boolean use3D_lma = clt_parameters.ilp.ilma_3d_lma;
		boolean use3D_lma_tilt_only = clt_parameters.ilp.ilma_3d_tilt_only;
		if (!ref_scene.hasGPU()) {
			throw new IllegalArgumentException ("interCorrPair(): CPU mode not supported");
		}
		TileProcessor tp = ref_scene.getTileProcessor();
		// Temporary reusing same ref scene ******
		boolean scene_is_ref_test =    clt_parameters.imp.scene_is_ref_test; // false; // true;
		boolean show_2d_correlations = clt_parameters.imp.show2dCorrelations(imp_debug_level); // true;
		boolean show_motion_vectors =  clt_parameters.imp.showMotionVectors(imp_debug_level); // true;
		boolean show_render_ref =      clt_parameters.imp.renderRef(imp_debug_level); // false; //true;
		boolean show_render_scene =    clt_parameters.imp.renderScene(imp_debug_level); // false; // true;
		boolean toRGB =                clt_parameters.imp.toRGB  ; // true;
		boolean show_coord_motion =    clt_parameters.imp.showCorrMotion(imp_debug_level); // mae its own
	    int     erase_clt = (toRGB? clt_parameters.imp.show_color_nan : clt_parameters.imp.show_mono_nan) ? 1:0;
	    boolean use_lma_dsi =          clt_parameters.imp.use_lma_dsi;
		boolean mov_en =               clt_parameters.imp.mov_en; // true;  // enable detection/removal of the moving objects during pose matching
	    boolean mov_debug_images =     clt_parameters.imp.showMovementDetection(imp_debug_level);
	    int mov_debug_level =          clt_parameters.imp.movDebugLevel(imp_debug_level);

	    boolean fpn_remove =           !fpn_disable && clt_parameters.imp.fpn_remove;
	    double  fpn_max_offset =       clt_parameters.imp.fpn_max_offset;
	    double  fpn_radius =           clt_parameters.imp.fpn_radius;
	    boolean fpn_ignore_border =    clt_parameters.imp.fpn_ignore_border; // only if fpn_mask != null - ignore tile if maximum touches fpn_mask
	    
	    boolean eq_debug =             false;
	    boolean transform_debug =      false;
	    boolean eq_en = near_important && clt_parameters.imp.eq_en; //  true;// equalize "important" FG tiles for better camera XYZ fitting	    
        int     eq_stride_hor =        clt_parameters.imp.eq_stride_hor; //  8;
		int     eq_stride_vert =       clt_parameters.imp.eq_stride_vert; // 8;
		double  eq_min_stile_weight =  clt_parameters.imp.eq_min_stile_weight; // 0.2; // 1.0;
		int     eq_min_stile_number =  clt_parameters.imp.eq_min_stile_number; // 10;
		double  eq_min_stile_fraction =clt_parameters.imp.eq_min_stile_fraction; // 0.02; // 0.05;
		double  eq_min_disparity =     clt_parameters.imp.eq_min_disparity; //  5;
		double  eq_max_disparity =     clt_parameters.imp.eq_max_disparity; // 100;
		double  eq_weight_add =        clt_parameters.imp.eq_weight_add; //  0.05;
		double  eq_weight_scale =      clt_parameters.imp.eq_weight_scale; // 10;
		double  eq_level =             clt_parameters.imp.eq_level; //  0.8; // equalize to (log) fraction of average/this strength      
	   
		final double scene_disparity_cor = clt_parameters.imp.disparity_corr; // 04/07/2023 // 0.0;		
		final double gpu_sigma_corr =     clt_parameters.getGpuCorrSigma(ref_scene.isMonochrome());
		final double gpu_sigma_rb_corr =  ref_scene.isMonochrome()? 1.0 : clt_parameters.gpu_sigma_rb_corr;
		final double gpu_sigma_log_corr = clt_parameters.getGpuCorrLoGSigma(ref_scene.isMonochrome());
		
		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; // 2.0;   // motion blur maximal gain (if more - move second point more than a pixel
		
		int tilesX =         tp.getTilesX();
		int tilesY =         tp.getTilesY();
		double [][][] coord_motion = null; // new double [2][tilesX*tilesY][];
		final double [][][] motion_vectors = show_motion_vectors?new double [tilesY *tilesX][][]:null;
		final float  [][][] fclt_corr = ((accum_2d_corr != null) || show_2d_correlations) ?
				(new float [tilesX * tilesY][][]) : null;
		if (debug_level > 1) {
			System.out.println("interCorrPair():   "+IntersceneLma.printNameV3("ATR",scene_atr)+
					" "+IntersceneLma.printNameV3("XYZ",scene_xyz));
		}
		ImageDtt image_dtt;
		image_dtt = new ImageDtt(
				ref_scene.getNumSensors(), // ,
				clt_parameters.transform_size,
				clt_parameters.img_dtt,
				ref_scene.isAux(),
				ref_scene.isMonochrome(),
				ref_scene.isLwir(),
				clt_parameters.getScaleStrength(ref_scene.isAux()),
				ref_scene.getGPU());
		image_dtt.getCorrelation2d(); // initiate image_dtt.correlation2d, needed if disparity_map != null  
		if (ref_scene.getGPU() != null) {
			ref_scene.getGPU().setGpu_debug_level(debug_level - 4); // monitor GPU ops >=-1
		}
		final double disparity_corr = clt_parameters.imp.disparity_corr; // 04/07/2023 // 0.0; // (z_correction == 0) ? 0.0 : geometryCorrection.getDisparityFromZ(1.0/z_correction);
//ref_disparity	
		if (ref_disparity == null) {
			ref_disparity = ref_scene.getDLS()[use_lma_dsi?1:0];
		}
		
		double [][] scene_pXpYD = OpticalFlow.transformToScenePxPyD( // will be null for disparity == NaN, total size - tilesX*tilesY
				null, // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
				ref_disparity, // dls[0], // final double []   disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
				scene_xyz,    // final double []   scene_xyz, // camera center in world coordinates
				scene_atr,    // final double []   scene_atr, // camera orientation relative to world frame
				scene,        // final QuadCLT     scene_QuadClt,
				scene_is_ref_test ? null: ref_scene);   // final QuadCLT     reference_QuadClt)
		double [][] scene_disparity_strength = null; // calculate if needed	
		if (use3D) { //(scene_disparity_strength != null)
			final boolean [] valid_tiles = new boolean[tilesX * tilesY]; // or use null?
			float [][][][] fcorr_td = new float[tilesY][tilesX][][];
			int mcorr_sel = Correlation2d.corrSelEncode(clt_parameters.img_dtt, scene.getNumSensors());
			double [][] disparity_map = new double [ImageDtt.getDisparityTitles(scene.getNumSensors(), scene.isMonochrome()).length][];
			TpTask[]  tp_tasks_disp =  GpuQuad.setInterTasks( // just to calculate valid_tiles
					scene.getNumSensors(),
					scene.getGeometryCorrection().getSensorWH()[0],
					!scene.hasGPU(), // final boolean             calcPortsCoordinatesAndDerivatives, // GPU can calculate them centreXY
					scene_pXpYD, // final double [][]         pXpYD, // per-tile array of pX,pY,disparity triplets (or nulls)
					null,          // final boolean []          selection, // may be null, if not null do not  process unselected tiles
					scene.getGeometryCorrection(), // final GeometryCorrection  geometryCorrection,
					scene_disparity_cor, // final double              disparity_corr,
	    			margin, // final int                 margin,      // do not use tiles if their centers are closer to the edges
	    			valid_tiles, // final boolean []          valid_tiles,            
	    			THREADS_MAX); // final int                 threadsMax)  // maximal number of threads to launch
			
			  image_dtt.quadCorrTD( // maybe remove "imageDtt."
					  clt_parameters.img_dtt,            // final ImageDttParameters imgdtt_params,    // Now just extra correlation parameters, later will include, most others
					  tp_tasks_disp, // *** will be updated inside from GPU-calculated geometry
					  fcorr_td, // fcorrs_td[nscene],                 // [tilesY][tilesX][pair][4*64] transform domain representation of 6 corr pairs
					  clt_parameters.gpu_sigma_r,        // 0.9, 1.1
					  clt_parameters.gpu_sigma_b,        // 0.9, 1.1
					  clt_parameters.gpu_sigma_g,        // 0.6, 0.7
					  clt_parameters.gpu_sigma_m,        //  =       0.4; // 0.7;
					  gpu_sigma_rb_corr,                 // final double              gpu_sigma_rb_corr, //  = 0.5; // apply LPF after accumulating R and B correlation before G, monochrome ? 1.0 : gpu_sigma_rb_corr;
					  gpu_sigma_corr,                    //  =    0.9;gpu_sigma_corr_m
					  gpu_sigma_log_corr,                // final double              gpu_sigma_log_corr,   // hpf to reduce dynamic range for correlations
					  clt_parameters.corr_red,           // +used
					  clt_parameters.corr_blue,          // +used
					  mcorr_sel,                         // final int                 mcorr_sel,    // Which pairs to correlate // +1 - all, +2 - dia, +4 - sq, +8 - neibs, +16 - hor + 32 - vert
					  THREADS_MAX,       // maximal number of threads to launch
					  debug_level);
			
			image_dtt.clt_process_tl_correlations( // convert to pixel domain and process correlations already prepared in fcorr_td and/or fcorr_combo_td
					clt_parameters.img_dtt,		   // final ImageDttParameters  imgdtt_params,   // Now just extra correlation parameters, later will include, most others
					fcorr_td,		      	       // final float  [][][][]     fcorr_td,        // [tilesY][tilesX][pair][4*64] transform domain representation of all selected corr pairs
					null, // num_acc,              // float [][][]                num_acc,         // number of accumulated tiles [tilesY][tilesX][pair] (or null)       
					null, // tile_corr_weights,             // dcorr_weight,                  // double []                 dcorr_weight,    // alternative to num_acc, compatible with CPU processing (only one non-zero enough)
					clt_parameters.gpu_corr_scale, //  final double              gpu_corr_scale,  //  0.75; // reduce GPU-generated correlation values
					clt_parameters.getGpuFatZero(scene.isMonochrome()),   // final double     gpu_fat_zero,    // clt_parameters.getGpuFatZero(is_mono);absolute == 30.0
					image_dtt.transform_size - 1,  // final int                 gpu_corr_rad,    // = transform_size - 1 ?
			        // The tp_tasks data should be decoded from GPU to get coordinates
					tp_tasks_disp,                  // final TpTask []           tp_tasks,        // data from the reference frame - will be applied to LMW for the integrated correlations
					null, // final double [][][]       far_fgbg,        // null, or [tilesY][tilesX]{disp(fg)-disp(bg), str(fg)-str(bg)} hints for LMA FG/BG split 
					scene.getGeometryCorrection().getRXY(false), // final double [][]         rXY,             // from geometryCorrection
					// next both can be nulls
					null,                          // final double [][][][]     clt_corr_out,   // sparse (by the first index) [type][tilesY][tilesX][(2*transform_size-1)*(2*transform_size-1)] or null
				    // combo will be added as extra pair if mcorr_comb_width > 0 and clt_corr_out has a slot for it
					// to be converted to float
					null, // dcorr_tiles,                   // final double  [][][]      dcorr_tiles,     // [tile][pair][(2*transform_size-1)*(2*transform_size-1)] // if null - will not calculate
					// When clt_mismatch is non-zero, no far objects extraction will be attempted
					false,                         // final boolean             use_rms, // DISPARITY_STRENGTH_INDEX means LMA RMS (18/04/2023)
					//optional, may be null
					disparity_map,                 // final double [][]         disparity_map,   // [8][tilesY][tilesX], only [6][] is needed on input or null - do not calculate
					null,                          // final double [][]         ddnd,            // data for LY. SHould be either null or [num_sensors][]
					use3D_lma, // run_lma, // clt_parameters.correlate_lma,  // final boolean             run_lma,         // calculate LMA, false - CM only
// define combining of all 2D correlation pairs for CM (LMA does not use them)
					clt_parameters.img_dtt.mcorr_comb_width, //final int                 mcorr_comb_width,  // combined correlation tile width (set <=0 to skip combined correlations)
					clt_parameters.img_dtt.mcorr_comb_height,//final int                 mcorr_comb_height, // combined correlation tile full height
					clt_parameters.img_dtt.mcorr_comb_offset,//final int                 mcorr_comb_offset, // combined correlation tile height offset: 0 - centered (-height/2 to height/2), height/2 - only positive (0 to height)
					clt_parameters.img_dtt.mcorr_comb_disp,	 //final double              mcorr_comb_disp,   // Combined tile per-pixel disparity for baseline == side of a square
					clt_parameters.clt_window,     // final int                 window_type,     // GPU: will not be used
					clt_parameters.tileX,          // final int                 debug_tileX,
					clt_parameters.tileY,          // final int                 debug_tileY,
					THREADS_MAX,                    // final int                 threadsMax,      // maximal number of threads to launch
					null,                          // final String              debug_suffix,
					debug_level); // + 2+1); // -1 );              // final int                 globalDebugLevel)\
			if (use3D_lma) {
				scene_disparity_strength=new double[][]{
					disparity_map[ImageDtt.DISPARITY_INDEX_POLY],
					disparity_map[ImageDtt.DISPARITY_INDEX_POLY+1]};
			} else {
				scene_disparity_strength=new double[][]{
					disparity_map[ImageDtt.DISPARITY_INDEX_CM],
					disparity_map[ImageDtt.DISPARITY_INDEX_CM+1]};
			}
			// remove disparity average, only detect tilts
			double average_disparity = 0;
			double average_absolute_disparity = 0;
			{
				double sw = 0, swd = 0;
				for (int i = 0; i < scene_pXpYD.length; i++) {
					if ((scene_pXpYD[i]!=null) && !Double.isNaN(scene_pXpYD[i][2])) {
						sw+=1.0;
						swd += scene_pXpYD[i][2];
					}
				}
				average_absolute_disparity = swd/sw;
				// remove average disparity and multiply by average disparity to have the same influence ratio to tild 
				sw = 0;
				swd = 0;
				for (int i = 0; i < scene_disparity_strength[0].length; i++) {
					if (Double.isNaN(scene_disparity_strength[0][i]) || Double.isNaN(scene_disparity_strength[1][i])) {
						scene_disparity_strength[0][i]=0;
						scene_disparity_strength[1][i]=0;
					}
					sw +=  scene_disparity_strength[1][i];
					swd += scene_disparity_strength[0][i] * scene_disparity_strength[1][i];
				}
				average_disparity = swd/sw;
				double magic_scale = use3D_lma? 1.0 : ref_scene.getTileProcessor().getMagicScale();
				for (int i = 0; i < scene_disparity_strength[0].length; i++) {
//					scene_disparity_strength[0][i] = (scene_disparity_strength[0][i] - average_disparity) / average_absolute_disparity;
					scene_disparity_strength[0][i] = (scene_disparity_strength[0][i] - average_disparity) /magic_scale;
					scene_disparity_strength[1][i] = 1.0 / average_absolute_disparity;
				}				
			}
		}
		if (transform_debug) {
			// calculate with no transform
			double [][] dbg_ref_pXpYD = OpticalFlow.transformToScenePxPyD( // will be null for disparity == NaN, total size - tilesX*tilesY
					null, // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
					ref_disparity, // dls[0], // final double []   disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
					ZERO3, // scene_xyz,    // final double []   scene_xyz, // camera center in world coordinates
					ZERO3, // scene_atr,    // final double []   scene_atr, // camera orientation relative to world frame
					ref_scene,    // final QuadCLT     scene_QuadClt,
					ref_scene);   // final QuadCLT     reference_QuadClt)
			double [][] dbg_ref_pXpYD2 = OpticalFlow.transformToScenePxPyD( // will be null for disparity == NaN, total size - tilesX*tilesY
					null, // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
					ref_disparity, // dls[0], // final double []   disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
					ZERO3, // scene_xyz,    // final double []   scene_xyz, // camera center in world coordinates
					new double[] {0,0,0.00001}, // scene_atr,    // final double []   scene_atr, // camera orientation relative to world frame
					ref_scene,    // final QuadCLT     scene_QuadClt,
					ref_scene);   // final QuadCLT     reference_QuadClt)
			double [][] dbg_img = new double[6][dbg_ref_pXpYD.length];
			for (int i = 0; i < dbg_img.length; i++) {
				Arrays.fill(dbg_img[i], Double.NaN);
			}
			for (int nt = 0; nt < dbg_ref_pXpYD.length; nt++) {
				if (dbg_ref_pXpYD[nt] != null) {
					for (int i = 0; i < dbg_ref_pXpYD[nt].length; i++){
						dbg_img[2*i][nt] = dbg_ref_pXpYD[nt][i];
					}
				}
				if (dbg_ref_pXpYD2[nt] != null) {
					for (int i = 0; i < dbg_ref_pXpYD2[nt].length; i++){
						dbg_img[2*i+1][nt] = dbg_ref_pXpYD2[nt][i];
					}
				}
			}
			String [] dbg_titles = {"pX0","pX2","pY0","pY2","disp0","disp2"};
			ShowDoubleFloatArrays.showArrays( // out of boundary 15
					dbg_img,
					tilesX,
					tilesY,
					true,
					scene.getImageName()+"-"+ref_scene.getImageName()+"-transform_test",
					dbg_titles);
			System.out.println("transform_test ref_scene: "+ ref_scene.getImageName());
			ref_scene.getErsCorrection().printVectors(scene_xyz, scene_atr);
		}
		
		
		TpTask[][] tp_tasks;
		if (mb_en && (mb_vectors!=null)) {
			tp_tasks  =  GpuQuad.setInterTasksMotionBlur(
					scene.getNumSensors(),
					scene.getErsCorrection().getSensorWH()[0],
					!scene.hasGPU(),          // final boolean             calcPortsCoordinatesAndDerivatives, // GPU can calculate them centreXY
					scene_pXpYD,              // final double [][]         pXpYD, // per-tile array of pX,pY,disparity triplets (or nulls)
					selection,                // final boolean []          selection, // may be null, if not null do not  process unselected tiles
					// motion blur compensation 
					mb_tau,                   // final double              mb_tau,      // 0.008; // time constant, sec
					mb_max_gain,              // final double              mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
					mb_vectors,               // final double [][]         mb_vectors,  //
					scene.getErsCorrection(), // final GeometryCorrection  geometryCorrection,
					disparity_corr,           // final double              disparity_corr,
					margin,                   // final int                 margin,      // do not use tiles if their centers are closer to the edges
					null,                     // final boolean []          valid_tiles,            
					THREADS_MAX);              // final int                 threadsMax)  // maximal number of threads to launch
		} else {
			tp_tasks = new TpTask[1][];
			tp_tasks[0]  =  GpuQuad.setInterTasks(
					scene.getNumSensors(),
					scene.getErsCorrection().getSensorWH()[0],
					!scene.hasGPU(),          // final boolean             calcPortsCoordinatesAndDerivatives, // GPU can calculate them centreXY
					scene_pXpYD,              // final double [][]         pXpYD, // per-tile array of pX,pY,disparity triplets (or nulls)
					selection,                // final boolean []          selection, // may be null, if not null do not  process unselected tiles
					scene.getErsCorrection(), // final GeometryCorrection  geometryCorrection,
					disparity_corr,           // final double              disparity_corr,
					margin,                   // final int                 margin,      // do not use tiles if their centers are closer to the edges
					null,                     // final boolean []          valid_tiles,            
					THREADS_MAX);              // final int                 threadsMax)  // maximal number of threads to launch
		}

		scene.saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
		float  [][][][]     fcorr_td =  null; // no accumulation, use data in GPU
		// Generate 2D phase correlations from the CLT representation
		// generates sum of the per-channel correlations as the last slot.
		// updates gpuQuad.gpu_corr_indices, gpuQuad.gpu_corrs_td and some other
		if (mb_en && (mb_vectors!=null)) {
			image_dtt.interCorrTDMotionBlur(
					clt_parameters.img_dtt,     // final ImageDttParameters  imgdtt_params,    // Now just extra correlation parameters, later will include, most others
					tp_tasks,                // final TpTask[][]            tp_tasks,
					fcorr_td,                   // final float  [][][][]     fcorr_td,        // [tilesY][tilesX][pair][4*64] transform domain representation of 6 corr pairs
					clt_parameters.gpu_sigma_r, // final double              gpu_sigma_r,     // 0.9, 1.1
					clt_parameters.gpu_sigma_b, // final double              gpu_sigma_b,     // 0.9, 1.1
					clt_parameters.gpu_sigma_g, // final double              gpu_sigma_g,     // 0.6, 0.7
					clt_parameters.gpu_sigma_m, // final double              gpu_sigma_m,     //  =       0.4; // 0.7;
					gpu_sigma_rb_corr,          // final double              gpu_sigma_rb_corr,    //  = 0.5; // apply LPF after accumulating R and B correlation before G, monochrome ? 1.0 :
					gpu_sigma_corr,             // final double              gpu_sigma_corr,       //  =    0.9;gpu_sigma_corr_m
					gpu_sigma_log_corr,         // final double              gpu_sigma_log_corr,   // hpf to reduce dynamic range for correlations
					clt_parameters.corr_red,    // final double              corr_red, // +used
					clt_parameters.corr_blue,   // final double              corr_blue,// +used
					sensor_mask_inter,          // final int                 sensor_mask_inter, // The bitmask - which sensors to correlate, -1 - all.
					THREADS_MAX,                 // final int                 threadsMax,       // maximal number of threads to launch
					debug_level);               // final int                 globalDebugLevel);
		} else {
			image_dtt.interCorrTD(
					clt_parameters.img_dtt,     // final ImageDttParameters  imgdtt_params,    // Now just extra correlation parameters, later will include, most others
					tp_tasks[0],                // final TpTask[]            tp_tasks,
					fcorr_td,                   // final float  [][][][]     fcorr_td,        // [tilesY][tilesX][pair][4*64] transform domain representation of 6 corr pairs
					clt_parameters.gpu_sigma_r, // final double              gpu_sigma_r,     // 0.9, 1.1
					clt_parameters.gpu_sigma_b, // final double              gpu_sigma_b,     // 0.9, 1.1
					clt_parameters.gpu_sigma_g, // final double              gpu_sigma_g,     // 0.6, 0.7
					clt_parameters.gpu_sigma_m, // final double              gpu_sigma_m,     //  =       0.4; // 0.7;
					gpu_sigma_rb_corr,          // final double              gpu_sigma_rb_corr,    //  = 0.5; // apply LPF after accumulating R and B correlation before G, monochrome ? 1.0 :
					gpu_sigma_corr,             // final double              gpu_sigma_corr,       //  =    0.9;gpu_sigma_corr_m
					gpu_sigma_log_corr,         // final double              gpu_sigma_log_corr,   // hpf to reduce dynamic range for correlations
					clt_parameters.corr_red,    // final double              corr_red, // +used
					clt_parameters.corr_blue,   // final double              corr_blue,// +used
					sensor_mask_inter,          // final int                 sensor_mask_inter, // The bitmask - which sensors to correlate, -1 - all.
					THREADS_MAX,                // final int                 threadsMax,       // maximal number of threads to launch
					debug_level);               // final int                 globalDebugLevel);
		}
		if (show_render_ref) {
			ImagePlus imp_render_ref = ref_scene.renderFromTD (
					-1,                  // final int         sensor_mask,
					false,               // boolean             merge_channels,
					clt_parameters,                                 // CLTParameters clt_parameters,
					clt_parameters.getColorProcParameters(ref_scene.isAux()), //ColorProcParameters colorProcParameters,
					clt_parameters.getRGBParameters(),              //EyesisCorrectionParameters.RGBParameters rgbParameters,
					null, // int []  wh,
					toRGB, // boolean toRGB,
					true, //boolean use_reference
					"GPU-SHIFTED-REF"); // String  suffix)
			imp_render_ref.show();
		}
		if (show_render_scene) { //set toRGB=false
			ImagePlus imp_render_scene = scene.renderFromTD (
					-1,                  // final int         sensor_mask,
					false,               // boolean             merge_channels,
					clt_parameters,                                 // CLTParameters clt_parameters,
					clt_parameters.getColorProcParameters(ref_scene.isAux()), //ColorProcParameters colorProcParameters,
					clt_parameters.getRGBParameters(),              //EyesisCorrectionParameters.RGBParameters rgbParameters,
					null, // int []  wh,
					toRGB, // boolean toRGB,
					false, //boolean use_reference
					"GPU-SHIFTED-SCENE"); // String  suffix)

			imp_render_scene.show();
		}
		if (dbg_corr_fpn != null) { // 2*16 or 2*17 (average, individual)
			float [][] foffsets = getInterCorrOffsetsDebug(
					tp_tasks_ref, // final TpTask[] tp_tasks_ref,
					tp_tasks[0], // final TpTask[] tp_tasks,
					ref_scene.getNumSensors(),
					tilesX, // final int      tilesX,
					tilesY); // final int      tilesY
			for (int i = 0; (i < dbg_corr_fpn.length) && (i < foffsets.length); i++) {
				dbg_corr_fpn[i] = foffsets[i];
			}
		}

		double [][] fpn_offsets = null;
		if (fpn_remove) {
			fpn_offsets = getInterCorrOffsets(
					fpn_max_offset, // final double   max_offset,
					tp_tasks_ref,   // final TpTask[] tp_tasks_ref,
					tp_tasks[0],       // final TpTask[] tp_tasks,
					ref_scene.getNumSensors(), // final int      numSens,
					tilesX,         // final int      tilesX,
					tilesY);        // final int      tilesY);
		}
		// Verify offsets before running LMA, return null and reason if test fails.
		if (min_max != null) {
			double [][] offsets = getInterCorrOffsets(
					Double.NaN, // final double   max_offset,
					tp_tasks_ref,   // final TpTask[] tp_tasks_ref,
					tp_tasks[0],       // final TpTask[] tp_tasks,
					ref_scene.getNumSensors(), // final int      numSens,
					tilesX,         // final int      tilesX,
					tilesY);        // final int      tilesY);
			if (offsets == null) {
				if (fail_reason != null) {
				    fail_reason[0]=FAIL_REASON_NULL;
				}
				return null;
			}
			double avg_offs2 = 0;
			double sw = 0;
			for (double [] offs:offsets) if (offs != null) {
				double w = 1.0;
				avg_offs2 += w * (offs[0] * offs[0] + offs[1] * offs[1]);
				sw += w;
			}
			if (sw == 0) {
				if (fail_reason != null) {
				    fail_reason[0]=FAIL_REASON_EMPTY;
				}
			}
			double avg_offs = Math.sqrt(avg_offs2 / sw);
			if (min_max.length>2) {
				min_max[2] = avg_offs;
			}
			if (avg_offs < min_max[0]) {// NaN - do not check;
				if (fail_reason != null) {
				    fail_reason[0]=FAIL_REASON_MIN;
				}
				return null;
			}
			
			if (avg_offs > min_max[1]) {// NaN - do not check;
				if (fail_reason != null) {
				    fail_reason[0]=FAIL_REASON_MAX;
				}
				if (debug_level > -3){
					System.out.println ("interCorrPair(): avg_offs = "+avg_offs+
								" > "+min_max[1]+", sw = "+sw);
				}

				return null;
			} else {
				if (debug_level > -3){
					System.out.println ("interCorrPair(): avg_offs = "+avg_offs+
								" <= "+min_max[1]+", sw = "+sw);
				}
			}
		}
		double            half_disparity = near_important ? 0.0 : clt_parameters.imp.half_disparity;
		double [][][]     dcorr_tiles = (fclt_corr != null)? (new double [tp_tasks[0].length][][]):null;
		// will use num_acc with variable number of accumulations (e.g. clusters)
		//all_fpn
		double min_str =      all_fpn ? clt_parameters.imp.min_str_fpn : clt_parameters.imp.min_str;
		double min_str_sum =  all_fpn ? clt_parameters.imp.min_str_sum_fpn : clt_parameters.imp.min_str_sum;
		double min_str_neib = all_fpn ? clt_parameters.imp.min_str_neib_fpn : clt_parameters.imp.min_str_neib;
		double corr_fz_inter = clt_parameters.getGpuFatZeroInter(ref_scene.isMonochrome());
		if (mb_en && (mb_vectors!=null)) { // increase fat zero when there is motion blur
			corr_fz_inter *= 8;
		}
		
		boolean use_neibs =        clt_parameters.imp.use_neibs;                  // false; // true;
		boolean use_neibs_pd =     true;		
		boolean neibs_nofpn_only = clt_parameters.imp.neibs_nofpn_only |
				(initial_adjust && clt_parameters.imp.neibs_nofpn_init);           // consolidate neighbors fot non-fpn tiles only!
		boolean redo_both =        clt_parameters.imp.redo_both;                  // use average of neighbors for both pd,td if any of the center tile tests (td, pd) fails
		int min_num_neibs =        clt_parameters.imp.min_num_neibs;              // plus center, total number >= (min_num_neibs+1)
		double scale_neibs_pd = use_neibs_pd? clt_parameters.imp.scale_neibs_pd : 0; // scale threshold for the pixel-domain average maximums		
		double scale_neibs_td = use_neibs_pd? clt_parameters.imp.scale_neibs_td : 0; // scale threshold for the transform-domain average maximums
		double scale_avg_weight =  clt_parameters.imp.scale_avg_weight;           // reduce influence of the averaged correlations compared to the single-tile ones
		
		int [] corr_indices_dbg = show_2d_correlations? image_dtt.getGPU().getCorrIndices() : null;
		boolean use_partial = clt_parameters.imp.use_partial;
		coord_motion = image_dtt.clt_process_tl_interscene(       // convert to pixel domain and process correlations already prepared in fcorr_td and/or fcorr_combo_td
				clt_parameters.img_dtt,            // final ImageDttParameters  imgdtt_params,   // Now just extra correlation parameters, later will include, most others
				// only used here to keep extra array element for disparity difference
				use3D,                             // boolean        use3D,         // generate disparity difference
				use_neibs,				           // boolean                   use_neibs,
				fcorr_td,                          // final float  [][][][]     fcorr_td,        // [tilesY][tilesX][pair][4*64] transform domain representation of all selected corr pairs
				null,                              // float [][][]              num_acc,         // number of accumulated tiles [tilesY][tilesX][pair] (or null). Can be inner null if not used in tp_tasks
				null,                              // double []                 dcorr_weight,    // alternative to num_acc, compatible with CPU processing (only one non-zero enough)
				clt_parameters.gpu_corr_scale,     // final double              gpu_corr_scale,  //  0.75; // reduce GPU-generated correlation values
				corr_fz_inter,                     // final double              gpu_fat_zero,    // clt_parameters.getGpuFatZero(is_mono);absolute == 30.0
				image_dtt.transform_size - 1,      // final int                 gpu_corr_rad,    // = transform_size - 1 ?
				// The tp_tasks data should be decoded from GPU to get coordinates
				// should it be reference or scene? Or any?
				tp_tasks[0],                       // final TpTask []           tp_tasks,        // data from the reference frame - will be applied to LMA for the integrated correlations
				// to be converted to float (may be null)
				dcorr_tiles,                       // final double  [][][]      dcorr_tiles,     // [tile][pair_abs, sparse][(2*transform_size-1)*(2*transform_size-1)] // if null - will not calculate
				pXpYD_ref, // ref_pXpYD,           // final double [][]         pXpYD,           // pXpYD for the reference scene
				fpn_offsets,                       // final double [][]         fpn_offsets,     // null, or per-tile X,Y offset to be blanked
				fpn_radius,                        // final double              fpn_radius,      // radius to be blanked around FPN offset center
				fpn_ignore_border,                 // final boolean             fpn_ignore_border, // only if fpn_mask != null - ignore tile if maximum touches fpn_mask			
				motion_vectors,                    // final double [][][]       motion_vectors,  // [tilesY*tilesX][][] -> [][][num_sel_sensors+1][2]
				clt_parameters.imp.run_poly,       // final boolean             run_poly,        // polynomial max, if false - centroid
				use_partial,                       // final boolean             use_partial,     // find motion vectors for individual pairs, false - for sum only
				clt_parameters.imp.centroid_radius,// final double              centroid_radius, // 0 - use all tile, >0 - cosine window around local max
				clt_parameters.imp.n_recenter,     // final int                 n_recenter,      // when cosine window, re-center window this many times
				clt_parameters.imp.td_weight,      // final double              td_weight,       // mix correlations accumulated in TD with 
				clt_parameters.imp.td_neib_weight, // final double              td_neib_weight,  // mix correlations accumulated in TD (neibs)  
				clt_parameters.imp.pd_weight,      // final double              pd_weight,       // correlations (post) accumulated in PD
				clt_parameters.imp.td_nopd_only,   // final boolean             td_nopd_only   , // only use TD accumulated data if no safe PD is available for the tile.
				clt_parameters.imp.neib_notd_only, // final boolean             neib_notd_only,  // use neighbors only if individual TD is too weak 
				min_str,                           // final double              min_str_nofpn,         //  = 0.25;
				min_str_sum,                       // final double              min_str_sum_nofpn,     // = 0.8; // 5;
				min_str_neib,                      // final double              min_str_neib_nofpn,
				clt_parameters.imp.min_str_fpn,    // final double              min_str,         //  = 0.25;
				clt_parameters.imp.min_str_sum_fpn,// final double              min_str_sum,     // = 0.8; // 5;
				clt_parameters.imp.min_str_neib_fpn,//final double              min_str_neib_fpn, 
				clt_parameters.imp.min_neibs,      // final int                 min_neibs,       //   2;	   // minimal number of strong neighbors (> min_str)
				clt_parameters.imp.weight_zero_neibs, // final double              weight_zero_neibs,//  0.2;   // Reduce weight for no-neib (1.0 for all 8)
				half_disparity,                    // final double              half_disparity,  //   5.0;   // Reduce weight twice for this disparity
				clt_parameters.imp.half_avg_diff,  // final double              half_avg_diff,   //   0.2;   // when L2 of x,y difference from average of neibs - reduce twice
				
				neibs_nofpn_only,                  // final boolean             neibs_nofpn_only, // consolidate neighbors fot non-fpn tiles only!
				redo_both,                         //final boolean             redo_both,        // use average of neighbors for both pd,td if any of the center tile tests (td, pd) fails
				min_num_neibs,                     //final int                 min_num_neibs,    // plus center, total number >= (min_num_neibs+1)
				scale_neibs_pd,                    //final double              scale_neibs_pd,   // scale threshold for the pixel-domain average maximums  		
				scale_neibs_td,                    //final double              scale_neibs_td,   // scale threshold for the transform-domain average maximums
				scale_avg_weight,                  //final double              scale_avg_weight,  // reduce influence of the averaged correlations compared to the single-tile ones
				
				clt_parameters.tileX,              // final int                 debug_tileX,
				clt_parameters.tileY,      	       // final int                 debug_tileY,
				THREADS_MAX,                       // final int                 threadsMax,       // maximal number of threads to launch
				debug_level);
		// optional coord_motion[1][3..4] is reserved for disparity difference and strength
		// final int                 globalDebugLevel);
		if (coord_motion == null) {
			System.out.println("clt_process_tl_interscene() returned null");
			if (fail_reason != null) {
			    fail_reason[0]=FAIL_REASON_INTERSCENE;
			}
			return null;
		}
		if (use3D) {//(scene_disparity_strength != null)
			// combine motion vector with disparity_diff/strength
			int num_slices = scene_disparity_strength.length;
			int coord_motion_slice = coord_motion.length - 1; // used for motionvector - last index
			for (int tile = 0; tile < coord_motion[coord_motion_slice].length; tile++ ) if ( coord_motion[coord_motion_slice][tile] != null){
				for (int i = 0; i < num_slices; i++) {
					coord_motion[coord_motion_slice][tile][i+3] = scene_disparity_strength[i][tile];
				}
			}
		}
		if (eq_en) {
			//		   		double  eq_weight_add = (min_str * clt_parameters.imp.pd_weight +  min_str_sum * clt_parameters.imp.td_weight) /
			//			   	   		 (clt_parameters.imp.pd_weight +  clt_parameters.imp.td_weight);
			double [] strength_backup = null;
			if (eq_debug) { // **** Set manually in debugger ****
				strength_backup = new double [coord_motion[1].length];
				for (int i = 0; i < strength_backup.length; i++) if (coord_motion[1][i] != null) {
					strength_backup[i] = coord_motion[1][i][2];
				}else {
					strength_backup[i] = Double.NaN;
				}
			}
			do {
				// restore
				if (strength_backup != null) {
					for (int i = 0; i < strength_backup.length; i++) if (coord_motion[1][i] != null) {
						coord_motion[1][i][2] = strength_backup[i];
					}
				}
				equalizeMotionVectorsWeights(
						coord_motion,          // final double [][][] coord_motion,
						tilesX,                // final int           tilesX,
						eq_stride_hor,         // final int           stride_hor,
						eq_stride_vert,        // final int           stride_vert,
						eq_min_stile_weight,   // final double        min_stile_weight,
						eq_min_stile_number,   // final int           min_stile_number,
						eq_min_stile_fraction, // final double        min_stile_fraction,
						eq_min_disparity,      // final double        min_disparity,
						eq_max_disparity,      // final double        max_disparity,
						eq_weight_add,         // final double        weight_add,
						eq_weight_scale,       // final double        weight_scale)
						eq_level);             // equalize to (log) fraction of average/this strength
				if (eq_debug) {
					String [] mvTitles = {"dx", "dy","conf", "conf0", "pX", "pY","Disp","defined"}; // ,"blurX","blurY", "blur"};
					double [][] dbg_img = new double [mvTitles.length][tilesX*tilesY];
					for (int l = 0; l < dbg_img.length; l++) {
						Arrays.fill(dbg_img[l], Double.NaN);
					}
					for (int nTile = 0; nTile <  coord_motion[0].length; nTile++) {
						if (coord_motion[0][nTile] != null) {
							for (int i = 0; i <3; i++) {
								dbg_img[4+i][nTile] = coord_motion[0][nTile][i];
							}
						}
						dbg_img[3] = strength_backup;
						if (coord_motion[1][nTile] != null) {
							for (int i = 0; i <3; i++) {
								dbg_img[0+i][nTile] = coord_motion[1][nTile][i];
							}
						}
						dbg_img[7][nTile] = ((coord_motion[0][nTile] != null)?1:0)+((coord_motion[0][nTile] != null)?2:0);
					}
					ShowDoubleFloatArrays.showArrays( // out of boundary 15
							dbg_img,
							tilesX,
							tilesY,
							true,
							scene.getImageName()+"-"+ref_scene.getImageName()+"-coord_motion-eq",
							mvTitles);

				}
			} while (eq_debug);
		}

		if (mov_en) {
			String debug_image_name = mov_debug_images ? (scene.getImageName()+"-"+ref_scene.getImageName()+"-movements"): null;
			boolean [] move_mask = getMovementMask(
					clt_parameters, // CLTParameters clt_parameters,
					coord_motion[1], // double [][]   motion, // only x,y,w components
					tilesX, // int           tilesX,
					debug_image_name, // String        debug_image_name,
					mov_debug_level); // int           debug_level);
			if (move_mask != null) {
				for (int nTile=0; nTile < move_mask.length; nTile++) if (move_mask[nTile]) {
					coord_motion[1][nTile]= null;
				}
			}
		}
		// seems that fclt_corr and fclt_corr1 are both not needed w/o debug
		if (show_2d_correlations) { // visualize prepare ref_scene correlation data
			float [][][] fclt_corr1 = ImageDtt.convertFcltCorr( // partial length, matching corr_indices = gpuQuad.getCorrIndices(); // also sets num_corr_tiles
					dcorr_tiles, // double [][][] dcorr_tiles,// [tile][sparse, correlation pair][(2*transform_size-1)*(2*transform_size-1)] // if null - will not calculate
					fclt_corr);  // float  [][][] fclt_corr) //  new float [tilesX * tilesY][][] or null
			if (use_neibs) {
				
			}
			float [][] dbg_corr_rslt_partial = ImageDtt.corr_partial_dbg( // not used in lwir
					fclt_corr1, // final float  [][][]     fcorr_data,       // [tile][pair][(2*transform_size-1)*(2*transform_size-1)] // if null - will not calculate
					corr_indices_dbg, // image_dtt.getGPU().getCorrIndices(), // tp_tasks,  // final TpTask []         tp_tasks,        //
					tilesX,    //final int                tilesX,
					tilesY,    //final int                tilesX,
					2*image_dtt.transform_size - 1,	// final int               corr_size,
					1000, // will be limited by available layersfinal int               layers0,
					clt_parameters.corr_border_contrast, // final double            border_contrast,
					THREADS_MAX, // final int               threadsMax,     // maximal number of threads to launch
					debug_level); // final int               globalDebugLevel)

			String [] titles = new String [dbg_corr_rslt_partial.length]; // dcorr_tiles[0].length];
			for (int i = 0; i < titles.length; i++) {
				if (i== (titles.length - 1)) {
					titles[i] = "sum";
				} else {
					titles[i] = "sens-"+i;
				}
			}

			// titles.length = 15, corr_rslt_partial.length=16!
			ShowDoubleFloatArrays.showArrays( // out of boundary 15
					dbg_corr_rslt_partial,
					tilesX*(2*image_dtt.transform_size),
					tilesY*(2*image_dtt.transform_size),
					true,
					scene.getImageName()+"-"+ref_scene.getImageName()+"-interscene",
					titles);
		}



		if (motion_vectors != null) {
			int num_sens = 0;
			int num_rslt = 0;
			find_num_layers:
				for (int nt = 0; nt <motion_vectors.length; nt++) {
					if (motion_vectors[nt] != null) {
						num_sens = motion_vectors[nt].length;
						for (int i = 0; i < num_sens; i++) {
							if (motion_vectors[nt][i] != null) {
								num_rslt = motion_vectors[nt][i].length;
								break find_num_layers;
							}
						}
					}
				}
			double [][] dbg_img = new double [num_sens * num_rslt][tilesX*tilesY];
			String [] titles = new String [dbg_img.length];
			for (int ns = 0; ns < num_sens; ns++) {
				String ss = (ns == (num_sens - 1))?"sum":(""+ns);
				for (int nr = 0; nr < num_rslt; nr++) {
					int indx = ns*num_rslt+nr;
					Arrays.fill(dbg_img[indx],Double.NaN);
					String sr="";
					switch (nr) {
					case 0:sr ="X";break;
					case 1:sr ="Y";break;
					case 2:sr ="S";break;
					default:
						sr = ""+nr;
					}
					titles[indx] = ss+":"+sr;
					for (int nt = 0; nt <motion_vectors.length; nt++) {
						if ((motion_vectors[nt] != null) && (motion_vectors[nt][ns] != null) ) {
							dbg_img[indx][nt] = motion_vectors[nt][ns][nr];
						}
					}
				}
			}
			ShowDoubleFloatArrays.showArrays( // out of boundary 15
					dbg_img,
					tilesX,
					tilesY,
					true,
					scene.getImageName()+"-"+ref_scene.getImageName()+"-motion_vectors",
					titles);
		}
		if (show_coord_motion) {
			//coord_motion
			String [] mvTitles = {"dx", "dy", "conf","disp_diff", "disp_str", "pX", "pY","Disp","defined"}; // ,"blurX","blurY", "blur"};
			double [][] dbg_img = new double [mvTitles.length][]; 
			for (int l = 0; l < dbg_img.length; l++) {
				if (use3D || (l < 3) || (l > 4) ) {
					dbg_img[l] = new double [tilesX*tilesY]; // keep unused null
					Arrays.fill(dbg_img[l], Double.NaN);
				}
			}
			for (int nTile = 0; nTile <  coord_motion[0].length; nTile++) {
				if (coord_motion[0][nTile] != null) {
					for (int i = 0; i < 3; i++) {
						dbg_img[5+i][nTile] = coord_motion[0][nTile][i];
					}
				}
				if (coord_motion[1][nTile] != null) {
					for (int i = 0; i < (use3D? 5 : 3); i++) {
						dbg_img[0+i][nTile] = coord_motion[1][nTile][i];
					}
				}
				dbg_img[8][nTile] = ((coord_motion[0][nTile] != null)?1:0)+((coord_motion[0][nTile] != null)?2:0);
			}
			ShowDoubleFloatArrays.showArrays( // out of boundary 15
					dbg_img,
					tilesX,
					tilesY,
					true,
					scene.getImageName()+"-"+ref_scene.getImageName()+"-coord_motion",
					mvTitles);
		}
		if (debug_level > 0){
			double  gap_frac = 0.25; 
			double [][] quad_strengths = getQuadStrengths(
					coord_motion,    // double [][][] coord_motion,
					gap_frac,        //double           gap_frac, // 0.25 
					tilesX);         // int           tilesX);
			
//			int num_defined = 0;
//			double sum_strength = 0.0;
			System.out.println ("interCorrPair(): quad_defined = ["+quad_strengths[0][0]+","
					+quad_strengths[0][1]+","+quad_strengths[0][2]+","+quad_strengths[0][3]+"]"
					+"), rel_strengths = ["
					+quad_strengths[1][0]+","+quad_strengths[1][1]+","
					+quad_strengths[1][2]+","+quad_strengths[1][3]+"] ");
		}
		if (fail_reason != null) {
			fail_reason[0]= 0;
		}
		return coord_motion; // here non-null
	}
	

	static float [][] getInterCorrOffsetsDebug(
			final TpTask[] tp_tasks_ref,
			final TpTask[] tp_tasks,
			final int      numSens,
			final int      tilesX,
			final int      tilesY
			){
		
		final int tiles = tilesX * tilesY;
		final float [][] offsets = new float [2*numSens+2][tiles];
		final int [] num_inp = new int[tiles];
		for (int i = 0; i < offsets.length; i++) {
			Arrays.fill(offsets[i], Float.NaN);
		}
		final Thread[] threads = ImageDtt.newThreadArray(THREADS_MAX);
		final AtomicInteger ai = new AtomicInteger(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile <tp_tasks.length; nTile = ai.getAndIncrement()) {
						TpTask task = tp_tasks[nTile];
						int tile = task.ty * tilesX + task.tx;
						num_inp[tile] = 1;
						for (int i = 0; i < numSens; i++) {
							offsets[2*i+2][tile] = task.xy[i][0];
							offsets[2*i+3][tile] = task.xy[i][1];
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		ai.set(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile <tp_tasks_ref.length; nTile = ai.getAndIncrement()) {
						TpTask task = tp_tasks_ref[nTile];
						int tile = task.ty * tilesX + task.tx;
						if (num_inp[tile] == 1) {
							num_inp[tile] = 2;
							float sx=0,sy=0;
							for (int i = 0; i < numSens; i++) {
								offsets[2*i+2][tile] -= task.xy[i][0];
								offsets[2*i+3][tile] -= task.xy[i][1];
								sx += offsets[2*i+2][tile];
								sy += offsets[2*i+3][tile];
							}
							offsets[0][tile] = sx/numSens;
							offsets[1][tile] = sy/numSens;
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return offsets;
	}
	/**
	 * Get average X,Y offsets between inter-scene correlated tiles
	 * to eliminate correlation between FPN of the same sensor.   
	 * @param max_offset maximal X, Y offset to keep (normally tile size == 8) 
	 * @param tp_tasks_ref reference tasks (should be updated from GPU to have 
	 *        actual X,Y for each sensor
	 * @param tp_tasks scene correlated to the reference, should also have
	 *        per-sensor coordinates
	 * @param numSens number of sensors
	 * @param tilesX number of tile columns
	 * @param tilesY number of tile rows.
	 * @return array of X, Y pairs ([tilesX*tilesY][2]), each may be null if 
	 *         not defined or out of tile. Returns null if no tiles contain a valid offset
	 */
	static double [][] getInterCorrOffsets(
			final double   max_offset,
			final TpTask[] tp_tasks_ref,
			final TpTask[] tp_tasks,
			final int      numSens,
			final int      tilesX,
			final int      tilesY
			){
		final int tiles = tilesX * tilesY;
		final double [][] offset_pairs = new double [2*numSens][tiles];
		final int [] num_inp = new int[tiles];
		for (int i = 0; i < offset_pairs.length; i++) {
			Arrays.fill(offset_pairs[i], Float.NaN);
		}
		double [][] offsets = new double[tiles][]; 
		final Thread[] threads = ImageDtt.newThreadArray(THREADS_MAX);
		final AtomicInteger ai = new AtomicInteger(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile <tp_tasks.length; nTile = ai.getAndIncrement()) {
						TpTask task = tp_tasks[nTile];
						int tile = task.ty * tilesX + task.tx;
						num_inp[tile] = 1;
						for (int i = 0; i < numSens; i++) {
							offset_pairs[2*i+0][tile] = task.xy[i][0];
							offset_pairs[2*i+1][tile] = task.xy[i][1];
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		final AtomicInteger anum_pairs = new AtomicInteger(0);
		ai.set(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile <tp_tasks_ref.length; nTile = ai.getAndIncrement()) {
						TpTask task = tp_tasks_ref[nTile];
						int tile = task.ty * tilesX + task.tx;
						if (num_inp[tile] == 1) {
							num_inp[tile] = 2;
							double sx=0,sy=0;
							for (int i = 0; i < numSens; i++) {
								sx += offset_pairs[2*i+0][tile] - task.xy[i][0];
								sy += offset_pairs[2*i+1][tile] - task.xy[i][1];
							}
							sx /= numSens;
							sy /= numSens;
//							if ((Math.abs(sx) <= max_offset) && (Math.abs(sy) <= max_offset )) {
							if (!(Math.abs(sx) > max_offset) && !(Math.abs(sy) > max_offset )) { // max_offset== NaN OK 
								offsets[tile] = new double[] {sx,sy};
								anum_pairs.getAndIncrement();
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		if (anum_pairs.get() > 0) {
			return offsets;
		}
		return null;
	}

	
	public static boolean [] getMovementMask(
			CLTParameters clt_parameters,
			double [][]   motion, // only x,y,w components
			int           tilesX,
			String        debug_image_name,
			int           debug_level)
	{
		boolean       show_debug_images = (debug_image_name != null);

		String [] mvTitles = {"dx", "dy", "conf", "blur", "blurX","blurY","clust","filtclust","re-clust","mask"};
		boolean mov_en =         clt_parameters.imp.mov_en; // true;  // enable detection/removal of the moving objects during pose matching
		if (!mov_en) {
			return null;
		}
		double  mov_sigma =      clt_parameters.imp.mov_sigma; // 1.5;   // pix - weighted-blur offsets before detection
		// next two to prevent motion detection while errors are too big
		double  mov_max_std =    clt_parameters.imp.mov_max_std; // 0.5;   // pix
		double  mov_thresh_rel = clt_parameters.imp.mov_thresh_rel; // 5.0;   // exceed average error
		double  mov_thresh_abs=  clt_parameters.imp.mov_thresh_abs; // 1.0;   // sqrt(dx^2+dy^2) in moving areas 
		double  mov_clust_max =  clt_parameters.imp.mov_clust_max; // 1.5;   // cluster maximum should exceed threshold this times
		int     mov_grow =       clt_parameters.imp.mov_grow; // 4;     // grow detected moving area
		int     mov_max_len =    clt_parameters.imp.mov_max_len; // 0 - no limit 
		
		double frac_clust =     0.25;//  cluster tiles compared to cluster max
		double  mov_max_std2 =   mov_max_std * mov_max_std;
		double  mov_thresh_rel2 = mov_thresh_rel* mov_thresh_rel;
		double  mov_thresh_abs2 = mov_thresh_abs * mov_thresh_abs;
		int    tilesY = motion.length / tilesX;
		double [] wa = new double[3];
		for (int nTile = 0; nTile <  motion.length; nTile++) if (motion[nTile] != null) {
			double w = motion[nTile][2]; //?  1.0; //
			wa[0]+=w;
			wa[1]+=w*motion[nTile][0];
			wa[2]+=w*motion[nTile][1];
		}
		wa[1] /= wa[0];
		wa[2] /= wa[0];
		double [][] mov_obj = new double[show_debug_images?4:2][tilesX*tilesY];
		double sl2 = 0;
		for (int nTile = 0; nTile <  motion.length; nTile++) if (motion[nTile] != null) {
			double w = motion[nTile][2]; //?  1.0; // 
			mov_obj[0][nTile]= w;
			double x = motion[nTile][0]-wa[1];
			double y = motion[nTile][1]-wa[2];
			double l2 = x*x + y*y;
			mov_obj[1][nTile]= w*l2;
			if (show_debug_images) {
				mov_obj[2][nTile]= w*x;
				mov_obj[3][nTile]= w*y;
			}
			sl2+=w*l2;
		}
		sl2/=wa[0];
		if (debug_level>0) {
			System.out.println("getMovementMask(): std="+Math.sqrt(sl2));
		}
		double [][] dbg_img = null;
		if(show_debug_images) {		
			dbg_img = new double [mvTitles.length][tilesX*tilesY];
			for (int l = 0; l < dbg_img.length; l++) {
				Arrays.fill(dbg_img[l], Double.NaN);
			}
		}		
		boolean [] move_mask = null; 
		double [] clust_max = null;
		int [][] minx_maxx_miny_maxy = null;
		process_movements:
		{	
			if (sl2 > mov_max_std2) {
				if (debug_level>0) {
					System.out.println("getMovementMask(): Too high scene mismatch: std="+Math.sqrt(sl2)+" > "+mov_max_std+", aborting movement processing");
				}
				break process_movements;
			}
			DoubleGaussianBlur gb = new DoubleGaussianBlur();
			gb.blurDouble(mov_obj[0], tilesX, tilesY, mov_sigma, mov_sigma, 0.01);
			gb.blurDouble(mov_obj[1], tilesX, tilesY, mov_sigma, mov_sigma, 0.01);
			for (int nTile = 0; nTile <  motion.length; nTile++) {
				mov_obj[1][nTile] /= mov_obj[0][nTile];
			}		
			if (show_debug_images) {
				gb.blurDouble(mov_obj[2], tilesX, tilesY, mov_sigma, mov_sigma, 0.01);
				gb.blurDouble(mov_obj[3], tilesX, tilesY, mov_sigma, mov_sigma, 0.01);
				for (int nTile = 0; nTile <  motion.length; nTile++) {
					mov_obj[2][nTile] /= mov_obj[0][nTile];
					mov_obj[3][nTile] /= mov_obj[0][nTile];
				}
				for (int nTile = 0; nTile <  motion.length; nTile++) {
					 if (motion[nTile] != null) {
						 dbg_img[0][nTile] =  motion[nTile][0];
						 dbg_img[1][nTile] =  motion[nTile][1];
						 dbg_img[2][nTile] =  motion[nTile][2];
					 }
					 dbg_img[3][nTile] =  mov_obj[1][nTile];				
					 dbg_img[4][nTile] =  mov_obj[2][nTile];				
					 dbg_img[5][nTile] =  mov_obj[3][nTile];
				}				
			}
			double mov_thresh = sl2 * mov_thresh_rel2;
			if (mov_thresh < mov_thresh_abs2) {
				mov_thresh = mov_thresh_abs2;
			}
			int num_over = 0;
			boolean [] over_thresh = new boolean [mov_obj[0].length];
			for (int nTile = 0; nTile <  motion.length; nTile++) if (mov_obj[1][nTile] > mov_thresh){
				over_thresh[nTile] = true;
				num_over ++;
			}
			if (num_over == 0) {
				if (debug_level>0) {
					System.out.println("getMovementMask(): No moving tiles exceed ="+Math.sqrt(mov_thresh)+", aborting movement processing");
				}
				break process_movements;
			}
			// Find clusters
			TileNeibs tn = new TileNeibs(tilesX,tilesY);

			int [] clusters = tn.enumerateClusters(
					over_thresh, // boolean [] tiles,
					null,        // int []     num_clusters,
					false);      // boolean ordered)
			// check clusters contain super-threshold tile
			double max_cluster = mov_thresh*mov_clust_max; // each cluster should have larger tile value to survive
			int max_clust_num = 0;
			for (int nTile=0; nTile < clusters.length;nTile++) if (clusters[nTile] > max_clust_num) {
				max_clust_num = clusters[nTile];
			}
			clust_max = new double [max_clust_num];
			for (int nTile=0; nTile < clusters.length;nTile++) if (clusters[nTile] > 0) {
				int i = clusters[nTile]-1;
				if (mov_obj[1][nTile] > clust_max[i]) {
					clust_max[i] = mov_obj[1][nTile];
				}
			}
			int num_good_clusters = 0;
			boolean [] good_clusters = new boolean[clust_max.length];
			for (int i = 0; i < clust_max.length; i++) if (clust_max[i] >= max_cluster){
				good_clusters[i] = true;
				num_good_clusters ++;
			}		
			if (debug_level>0) {
				System.out.println("getMovementMask(): Got "+max_clust_num+" clusters, of them good (>"+max_cluster+") - "+num_good_clusters );
				for (int i = 0; i < clust_max.length; i++) {
					System.out.println("getMovementMask(): Cluster "+i+(good_clusters[i]?"*":" ")+", max = "+clust_max[i]);
				}
			}
			
			if(show_debug_images) {		
				for (int nTile = 0; nTile <  motion.length; nTile++) if (clusters[nTile] > 0){
					 dbg_img[6][nTile] = clusters[nTile];
				}
			}
			// remove tiles much lower than cluster max
			for (int nTile=0; nTile < clusters.length;nTile++) if (clusters[nTile] > 0) {
				int iclust = clusters[nTile] - 1;
				double threshold = clust_max[iclust] * frac_clust;
				if (!good_clusters[iclust] || (mov_obj[1][nTile] < threshold)) {
					clusters[nTile] = 0;
				}
			}
			
			if(show_debug_images) {		
				for (int nTile = 0; nTile <  motion.length; nTile++) if (clusters[nTile] > 0){
					 dbg_img[7][nTile] = clusters[nTile];
				}
			}
			
			if (num_good_clusters == 0) {
				if (debug_level>0) {
					System.out.println("getMovementMask(): No strong enough moving clusters (of total "+clust_max.length+"), aborting movement processing");
				}
				break process_movements;
			}
			
			move_mask = new boolean [clusters.length];
			for (int nTile=0; nTile < clusters.length; nTile++) if (clusters[nTile] > 0) {
				move_mask[nTile] = good_clusters[clusters[nTile] -1];
			}
			
			tn.growSelection(
					mov_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
					move_mask, // boolean [] tiles,
					null);     // boolean [] prohibit);
			
			// Re-clusterize

			clusters = tn.enumerateClusters(
					move_mask, // boolean [] tiles,
					null,      // int []     num_clusters,
					false);    // boolean ordered)
			if(show_debug_images) {		
				for (int nTile = 0; nTile <  motion.length; nTile++) if (clusters[nTile] > 0){
					 dbg_img[8][nTile] = clusters[nTile];
				}
			}
			
			// Measure cluster sizes and remove too big ones
			if (mov_max_len > 0) {
				max_clust_num = 0;
				for (int nTile=0; nTile < clusters.length;nTile++) if (clusters[nTile] > max_clust_num) {
					max_clust_num = clusters[nTile];
				}
				minx_maxx_miny_maxy = new int [max_clust_num][];
				for (int nTile = 0; nTile <  motion.length; nTile++) if (clusters[nTile] > 0){
					int x = nTile % tilesX;
					int y = nTile / tilesX;
					int iclust = clusters[nTile] - 1;
					if (minx_maxx_miny_maxy[iclust] == null) {
						minx_maxx_miny_maxy[iclust] = new int[] {x,x,y,y};
					}
					if (x < minx_maxx_miny_maxy[iclust][0]) minx_maxx_miny_maxy[iclust][0] = x;
					if (x > minx_maxx_miny_maxy[iclust][1]) minx_maxx_miny_maxy[iclust][1] = x;
					if (y < minx_maxx_miny_maxy[iclust][2]) minx_maxx_miny_maxy[iclust][2] = y;
					if (y > minx_maxx_miny_maxy[iclust][3]) minx_maxx_miny_maxy[iclust][3] = y;
				}
				
				num_good_clusters = 0;
				good_clusters = new boolean[max_clust_num];
				for (int iclust = 0; iclust < max_clust_num; iclust++) {
					if (    (minx_maxx_miny_maxy[iclust] != null) &&
							((minx_maxx_miny_maxy[iclust][1] - minx_maxx_miny_maxy[iclust][0]) <= mov_max_len) &&
							((minx_maxx_miny_maxy[iclust][3] - minx_maxx_miny_maxy[iclust][2]) <= mov_max_len)) {
						good_clusters[iclust] = true;
						num_good_clusters ++;
					} else {
						minx_maxx_miny_maxy[iclust] = null;
					}
				}
				if (num_good_clusters == 0) {
					if (debug_level>0) {
						System.out.println("getMovementMask(): No small enough moving clusters (of total "+clust_max.length+"), aborting movement processing");
					}
					break process_movements;
				}
				
				if (debug_level>0) { // -1) {
					System.out.println("getMovementMask(): Got "+max_clust_num+" clusters, of them good - "+num_good_clusters );
					for (int i = 0; i < minx_maxx_miny_maxy.length; i++) {
						int sizex = minx_maxx_miny_maxy[i][1] - minx_maxx_miny_maxy[i][0];
						int sizey = minx_maxx_miny_maxy[i][3] - minx_maxx_miny_maxy[i][2];
						int max_size =  (sizex>sizey)? sizex:sizey;
						System.out.println("getMovementMask(): Cluster "+i+(good_clusters[i]?"*":" ")+
								", max_size="+max_size+" ("+sizex+", "+sizey+")"+
								", xmin = "+minx_maxx_miny_maxy[i][0]+
								", xmax = "+minx_maxx_miny_maxy[i][1]+
								", ymin = "+minx_maxx_miny_maxy[i][2]+
								", ymax = "+minx_maxx_miny_maxy[i][3]);
					}
				}
				// remove too large clusters
				if (mov_max_len > 0) {
					for (int nTile=0; nTile < clusters.length; nTile++) if (clusters[nTile] > 0) {
						int iclust = clusters[nTile] - 1;
						if (!good_clusters[iclust]) {
							clusters[nTile] = 0;
						}
					}
				}
				move_mask = new boolean [clusters.length];
				for (int nTile=0; nTile < clusters.length; nTile++) if (clusters[nTile] > 0) {
					move_mask[nTile] = good_clusters[clusters[nTile] -1];
				}
			}
			
			if(show_debug_images) {		
				for (int nTile = 0; nTile <  motion.length; nTile++) if (move_mask[nTile]){
					 dbg_img[9][nTile] = 1.0;
				}
			}

		}
		if(show_debug_images) {	
			ShowDoubleFloatArrays.showArrays( // out of boundary 15
					dbg_img,
					tilesX,
					tilesY,
					true,
					debug_image_name,
					mvTitles);
		}
		if (debug_level > -2) { // 0
			if (move_mask != null) {
				int num_mov = 0;
				for (int nTile=0; nTile<move_mask.length;nTile++) if (move_mask[nTile]) {
					num_mov++;
				}
				System.out.print("getMovementMask(): Created moving objects mask of "+num_mov+" tiles. Sizes:");
				if (minx_maxx_miny_maxy != null) {
					for (int iclust = 0; iclust < minx_maxx_miny_maxy.length; iclust++) if (minx_maxx_miny_maxy[iclust] != null){
						int sizex = minx_maxx_miny_maxy[iclust][1] - minx_maxx_miny_maxy[iclust][0];
						int sizey = minx_maxx_miny_maxy[iclust][3] - minx_maxx_miny_maxy[iclust][2];
						int max_size =  (sizex>sizey)? sizex:sizey;
						System.out.print(" "+max_size);
					}
				}
				System.out.println();

			} else {
				System.out.println("getMovementMask(): No moving objects mask is created.");
			}
		}
		return move_mask;
	}
	
	/**
	 * Equalize weights of the motion vectors to boost that of important buy weak one.
	 * Process overlapping (by half, using shifted cosine weight function) supertiles
	 * independently and if it qualifies, increase its tiles weights equlaizing (with
	 * certain limitations) total supertile weights.
	 * @param coord_motion       [2][tilesX*tilesY][3] input/output arrays.[0][tile][] is
	 *                           pXpYD triplet (
	 * @param stride_hor         half of a supertile width
	 * @param stride_vert        half of a supertile height
	 * @param min_stile_weight   minimal total weight of the tiles in a supertile (lower
	 *                           will not be modified)
	 * @param min_stile_number   minimal number of defined tiles in a supertile
	 * @param min_stile_fraction minimal total tile strength compared to the average one
	 * @param min_disparity      minimal disparity of tiles to consider (after filtering)
	 * @param max_disparity      maximal disparity of tiles to consider (after filtering)
	 * @param weight_add         add to each tile after scaling (if total weight < average)
	 * @param weight_scale       scale each tile (if total weight < average)
	 * @param eq_level           equalize (log scale) to this ratio between average and this
	 *                           strength (0.0 - leave as is, equalize weak to average strength 
	 * If total new weight of a supertile exceeds average - scale each tile to match. If
	 * lower - keep as is. Only after this step remove tiles (replace with original weight)
	 * that are discarded by the disparity filter. Multiply result by the the window and
	 * accumulate (in 4 passes to prevent contentions for the same destination array  
	 */
	
	public static void equalizeMotionVectorsWeights(
			final double [][][] coord_motion,
			final int           tilesX,
			final int           stride_hor,
			final int           stride_vert,
			final double        min_stile_weight,
			final int           min_stile_number,
			final double        min_stile_fraction,
			final double        min_disparity,
			final double        max_disparity,
			final double        weight_add,
			final double        weight_scale,
			final double        eq_level) // equalize to (log) fraction of average/this strength      
	{
		final int tiles = coord_motion[0].length;
		final int tilesY = tiles/tilesX;
		final int stile_width =  2 * stride_hor;
		final int stile_height = 2 * stride_vert;
		double [] whor =  new double [stride_hor];
		double [] wvert = new double [stride_vert];
		for (int i = 0; i < stride_hor;  i++) whor[i] =  0.5 *(1.0 - Math.cos(Math.PI*(i+0.5)/stride_hor)); 
		for (int i = 0; i < stride_vert; i++) wvert[i] = 0.5 *(1.0 - Math.cos(Math.PI*(i+0.5)/stride_vert));
		final int stilesX = (tilesX - 1)/stride_hor + 2; // 10  // 9
		final int stilesY = (tilesY - 1)/stride_vert + 2; // 8 // 7
		final int stiles = stilesX * stilesY;
		int indx = 0;
		final double[] wind = new double[stile_height*stile_width];
		for (int iy = 0; iy < stile_height;iy++) {
			int iy1 = (iy >= stride_vert) ? (stile_height - 1 - iy) : iy;
			for (int ix = 0; ix < stile_width; ix++) {
				int ix1 = (ix >= stride_hor) ? (stile_width - 1 - ix) : ix;
				wind[indx++] = wvert[iy1] * whor[ix1];
			}
		}
		final double [] stile_weight = new double [stiles];
		final int [] stile_number = new int [stiles];
		final Thread[] threads = ImageDtt.newThreadArray(THREADS_MAX);
		final AtomicInteger ai = new AtomicInteger(0);
		final int dbg_stile =  -1;
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int sTile = ai.getAndIncrement(); sTile < stiles; sTile = ai.getAndIncrement()) {
						if (sTile == dbg_stile) {
							System.out.println("normalizeMotionVectorsWeights (): dbg_stile = "+dbg_stile);
						}
						int sTileX = sTile % stilesX;
						int sTileY = sTile / stilesX;
						stile_weight[sTile] = 0.0;
						int num_tiles = 0; // for partial stiles
						for (int iy = 0; iy < stile_height;iy++) {
							int tileY = (sTileY - 1) * stride_vert + iy;
							if ((tileY < 0) || (tileY >= tilesY)) continue;
							for (int ix = 0; ix < stile_width; ix++) {
								int tileX = (sTileX - 1) * stride_hor + ix;
								if ((tileX < 0) || (tileX >= tilesX)) continue;
								num_tiles++; // for partial stiles
								int tile = tileX + tilesX*tileY;
								if ((coord_motion[1][tile] != null) && !Double.isNaN(coord_motion[0][tile][2])) {
									stile_weight[sTile] += coord_motion[1][tile][2];
									stile_number[sTile]++;
								}
							}
						}
						stile_weight[sTile] *= 1.0 * stile_height * stile_height / num_tiles; // increase for partiaL stiles
						stile_number[sTile] *= stile_height*stile_height;
						stile_number[sTile] /= num_tiles;
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		double sum_stile_str = 0.0;
		int num_stiles_defined = 0;
		for (int sTile = 0; sTile < stiles; sTile++) {
			if (stile_weight[sTile] > 0) {
				num_stiles_defined++;
				sum_stile_str+=stile_weight[sTile];
			}
		}
		if (num_stiles_defined <=0) {
			System.out.println("normalizeMotionVectorsWeights(): no defined supertiles, bailing out");
			return;
		}
		final boolean [] dbg_mod = new boolean [stiles];
		final double [] tile_new_strength = new double [tiles]; // accumulate new strengths here
		final double avg_stile_str = sum_stile_str / num_stiles_defined;
		final double min_combo_weight = Math.max(min_stile_weight, min_stile_fraction * avg_stile_str);
		for (int offsy = 0; offsy < 2; offsy++) { // avoiding overlap
			final int foffsy = offsy;
			final int sty2 = (stilesY + 1 - offsy) / 2;
			for (int offsx = 0; offsx < 2; offsx++) {
				final int foffsx = offsx;
				final int stx2 = (stilesX + 1 - offsx) / 2;
				final int st2 = sty2*stx2;
				ai.set(0);
				for (int ithread = 0; ithread < threads.length; ithread++) {
					threads[ithread] = new Thread() {
						public void run() {
							double [] mod_weights =  new double [stile_height*stile_width];
							for (int indx = ai.getAndIncrement(); indx < st2; indx = ai.getAndIncrement()) {
								int stileY = (indx/stx2)*2+foffsy;
								int stileX = (indx%stx2)*2+foffsx;
								int sTile = stileX + (stilesX * stileY);
								if (sTile == dbg_stile) {
									System.out.println("normalizeMotionVectorsWeights (): dbg_stile = "+dbg_stile);
								}
								boolean keep_old = false;
								// check this tile qualifies:
								//stile_number
								if (stile_number[sTile] < min_stile_number) {
									keep_old = true;
								}
								if (stile_weight[sTile] < min_combo_weight) {
									keep_old = true;
								}
								if (stile_weight[sTile] >= avg_stile_str) { // already strong enough
									keep_old = true;
								}
								double target_str=Math.exp(
										eq_level*Math.log(avg_stile_str) +
										(1.0-eq_level)*Math.log(stile_weight[sTile]));
								Arrays.fill(mod_weights,0);
								double sum_weights = 0.0;
								int num_tiles = 0; // for partial stiles
								for (int iy = 0; iy < stile_height;iy++) {
									int tileY = (stileY - 1) * stride_vert + iy;
									if ((tileY < 0) || (tileY >= tilesY)) continue;
									for (int ix = 0; ix < stile_width; ix++) {
										int tileX = (stileX - 1) * stride_hor + ix;
										if ((tileX < 0) || (tileX >= tilesX)) continue;
										int tile = tileX + tilesX*tileY;
										num_tiles ++;
										if ((coord_motion[1][tile] != null) && !Double.isNaN(coord_motion[0][tile][2])) {
											int ltile = ix + iy * stile_width;
											mod_weights[ltile] = keep_old ?
													coord_motion[1][tile][2] :
														(weight_add + coord_motion[1][tile][2] ) * weight_scale;
											sum_weights += mod_weights[ltile];
										}
									}
								}
								sum_weights *= 1.0 * stile_height * stile_height / num_tiles  ; // increase for partial tiles 
								if (!keep_old) {
									if (sum_weights > target_str) { // scale back
										double s = target_str/sum_weights;
										for (int ltile = 0; ltile < mod_weights.length; ltile++) {
											mod_weights[ltile] *= s;
										}
									}
									for (int iy = 0; iy < stile_height;iy++) {
										int tileY = (stileY - 1) * stride_vert + iy;
										if ((tileY < 0) || (tileY >= tilesY)) continue;
										for (int ix = 0; ix < stile_width; ix++) {
											int tileX = (stileX - 1) * stride_hor + ix;
											if ((tileX < 0) || (tileX >= tilesX)) continue;
											int tile = tileX + tilesX*tileY;
											int ltile = ix + iy * stile_width;
											// remove out of range disparity
											if (coord_motion[0][tile] != null) {
												double disp = coord_motion[0][tile][2]; // shopuld be non-null
												if ((disp < min_disparity) || (disp > max_disparity)) { // min/max = NaN - OK
													mod_weights[ltile] = coord_motion[1][tile][2]; // use original value
												}
											}
										}
									}
									dbg_mod[sTile] = true;
								}
								// multiply by window and accumulate
								for (int iy = 0; iy < stile_height;iy++) {
									int tileY = (stileY - 1) * stride_vert + iy;
									if ((tileY < 0) || (tileY >= tilesY)) continue;
									for (int ix = 0; ix < stile_width; ix++) {
										int tileX = (stileX - 1) * stride_hor + ix;
										if ((tileX < 0) || (tileX >= tilesX)) continue;
										int tile = tileX + tilesX*tileY;
										int ltile = ix + iy * stile_width;
										// wind
										tile_new_strength[tile] += wind[ltile] * mod_weights[ltile];
									}
								}
							}
						}
					};
				}		      
				ImageDtt.startAndJoin(threads);
			}
		}
		ai.set(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile < tiles; nTile = ai.getAndIncrement()) {
						if (tile_new_strength[nTile] > 0.0) { // assuming ((coord_motion[1][nTile] != null) && !Double.isNaN(coord_motion[0][nTile][2]))
							if (coord_motion[1][nTile] == null) {
								System.out.println("coord_motion[1]["+nTile+"] == null, tileX="+(nTile%tilesX)+", tileY="+(nTile/tilesX));
							} else {
								coord_motion[1][nTile][2] = tile_new_strength[nTile]; // replace modified
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return;
	}
	
	   /**
     * Calculates data availability in 4 corners (excluding center gap)
     * To disable ERS calculation in LMA 
     * @param coord_motion interCorrPair() output - here only defined (non-null)
     *                     and strength are used
     * @param gap_frac     fraction of width and height to remove in the center
     *                     0.25 - remove from 3/8 to 5/8 "cross" in the center
     * @param tilesX       number of tiles in a row
     * @return             [2][4] - [0][]: number of non-null tiles in each quadrant,
     *                     [1][] - fraction of weight of total weight (total does not exclude center)  
     */
	public static double [][] getQuadStrengths(
			double [][][] coord_motion,
			double        gap_frac, // 4 
			int           tilesX) {
		int tilesY = coord_motion[1].length/ tilesX;
		int num_defined = 0;
		double sum_strength = 0.0;
		double [][] quad_strengths = new double[2][4];
		int igapX = (int) (gap_frac * tilesX), igapY = (int) (gap_frac * tilesY);
		int igapX0 = (tilesX - igapX)/2;
		int igapX1 = igapX0 + igapX; 
		int igapY0 = (tilesY - igapY)/2;
		int igapY1 = igapY0 + igapY; 
		
		for (int i = 0; i < coord_motion[1].length; i++) if (coord_motion[1][i] != null){
			sum_strength += coord_motion[1][i][2];
			num_defined++;
			int ty = i / tilesX;
			int tx = i % tilesX;
			if (((ty > igapY0) && (ty < igapY1)) || ((tx > igapX0) && (tx < igapX1))) {
				continue;
			}
			int iy = ty/(tilesY/2);
			int ix = tx/(tilesX/2);
			int ixy = 2*iy+ix;
			quad_strengths[0][ixy] += 1.0;
			quad_strengths[1][ixy] += coord_motion[1][i][2];
		}
		if (sum_strength > 0) {
			for (int i = 0; i< quad_strengths[1].length; i++) {
//				quad_strengths[0][i]/=num_defined;
				quad_strengths[1][i]/=sum_strength;
			}
		}
		return quad_strengths;
	}
	/**
	 * Prepare and set GPU reference TD data to be used for interscene correlatiuon. Optionally uses
	 * motion blur correction (if mb_vectors != null)
	 * @param clt_parameters general parameters
	 * @param ref_scene     reference scene QuadCLT instance
	 * @param ref_disparity either alternative disparity array or null to use it from the reference scene itself
	 * @param ref_pXpYD - precalculated or null (will be calculated)
	 * @param selection optional selection to ignore unselected tiles)
	 * @param margin do not use tiles with centers closer than this to the edges. Measured in pixels.
	 * @param mb_tau Sensor time constant in seconds (only needed if mb_vectors != null)
	 * @param mb_max_gain maximal gain fro MB correction (higher MB corrected by increasing offset)
	 * @param mb_vectors [2][tiles] {dx/dt[tiles],dx/dt[tiles]} motion blur vectors or null 
	 * @param debug_level debug level
	 * @return TpTask[][] arrays used to program GPU. With no MB has only one element, with MB - two
	 */
	public static TpTask[][] setReferenceGPU (
			CLTParameters      clt_parameters,			
			QuadCLT            ref_scene,
			double []          ref_disparity, // null or alternative reference disparity
			double [][]        ref_pXpYD,
			final boolean []   selection, // may be null, if not null do not  process unselected tiles
			final int          margin,
			// motion blur compensation 
			double             mb_tau,      // 0.008; // time constant, sec
			double             mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
			double [][]        mb_vectors,  // now [2][ntiles];
			int                debug_level)
	{
		if (!ref_scene.hasGPU()) {
			throw new IllegalArgumentException ("setReferenceGPU(): CPU mode not supported");
		}
		boolean toRGB =       clt_parameters.imp.toRGB  ; // true;
	    int     erase_clt =   (toRGB? clt_parameters.imp.show_color_nan : clt_parameters.imp.show_mono_nan) ? 1:0;
	    boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;

		if (ref_scene.getGPU() != null) {
			ref_scene.getGPU().setGpu_debug_level(debug_level - 4); // monitor GPU ops >=-1
		}
		final double disparity_corr = clt_parameters.imp.disparity_corr; // 04/07/2023 // 0.0; // (z_correction == 0) ? 0.0 : geometryCorrection.getDisparityFromZ(1.0/z_correction);
//ref_disparity	
		if (ref_disparity == null) {
			ref_disparity = ref_scene.getDLS()[use_lma_dsi?1:0];
		}
		if (ref_pXpYD == null) {
			ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
					null, // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
					ref_disparity, // dls[0],  // final double []   disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
					ZERO3,         // final double []   scene_xyz, // camera center in world coordinates
					ZERO3,         // final double []   scene_atr, // camera orientation relative to world frame
					ref_scene,     // final QuadCLT     scene_QuadClt,
					ref_scene,    // final QuadCLT     reference_QuadClt)
					QuadCLT.THREADS_MAX);
		}
		ImageDtt image_dtt = new ImageDtt(
				ref_scene.getNumSensors(),
				clt_parameters.transform_size,
				clt_parameters.img_dtt,
				ref_scene.isAux(),
				ref_scene.isMonochrome(),
				ref_scene.isLwir(),
				clt_parameters.getScaleStrength(ref_scene.isAux()),
				ref_scene.getGPU());
        TpTask[][] tp_tasks_ref;
		if (mb_vectors!=null) {
			tp_tasks_ref = GpuQuad.setInterTasksMotionBlur( // "true" reference, with stereo actual reference will be offset
					ref_scene.getNumSensors(),
					ref_scene.getErsCorrection().getSensorWH()[0],
					!ref_scene.hasGPU(),          // final boolean             calcPortsCoordinatesAndDerivatives, // GPU can calculate them centreXY
					ref_pXpYD,                    // final double [][]         pXpYD, // per-tile array of pX,pY,disparity triplets (or nulls)
					selection,                    // final boolean []          selection, // may be null, if not null do not  process unselected tiles
					// motion blur compensation 
					mb_tau,                       // final double              mb_tau,      // 0.008; // time constant, sec
					mb_max_gain,                  // final double              mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
					mb_vectors,                   //final double [][]         mb_vectors,  //
					ref_scene.getErsCorrection(), // final GeometryCorrection  geometryCorrection,
					disparity_corr,               // final double              disparity_corr,
					margin,                       // final int                 margin,      // do not use tiles if their centers are closer to the edges
					null,                         // final boolean []          valid_tiles,            
					QuadCLT.THREADS_MAX);                  // final int                 threadsMax)  // maximal number of threads to launch
			ref_scene.saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
			image_dtt.setReferenceTDMotionBlur( // tp_tasks_ref will be updated
					erase_clt,
					null,                       // final int []              wh,               // null (use sensor dimensions) or pair {width, height} in pixels
					clt_parameters.img_dtt,     // final ImageDttParameters  imgdtt_params,    // Now just extra correlation parameters, later will include, most others
					true, // final boolean             use_reference_buffer,
					tp_tasks_ref,               // final TpTask[]            tp_tasks,
					clt_parameters.gpu_sigma_r, // final double              gpu_sigma_r,     // 0.9, 1.1
					clt_parameters.gpu_sigma_b, // final double              gpu_sigma_b,     // 0.9, 1.1
					clt_parameters.gpu_sigma_g, // final double              gpu_sigma_g,     // 0.6, 0.7
					clt_parameters.gpu_sigma_m, // final double              gpu_sigma_m,     //  =       0.4; // 0.7;
					QuadCLT.THREADS_MAX,                 // final int                 threadsMax,       // maximal number of threads to launch
					debug_level);               // final int                 globalDebugLevel);
			
		} else {
			tp_tasks_ref = new TpTask[1][];
			tp_tasks_ref[0] =  GpuQuad.setInterTasks( // "true" reference, with stereo actual reference will be offset
					ref_scene.getNumSensors(),
					ref_scene.getErsCorrection().getSensorWH()[0],
					!ref_scene.hasGPU(),          // final boolean             calcPortsCoordinatesAndDerivatives, // GPU can calculate them centreXY
					ref_pXpYD,                    // final double [][]         pXpYD, // per-tile array of pX,pY,disparity triplets (or nulls)
					selection,                    // final boolean []          selection, // may be null, if not null do not  process unselected tiles
					ref_scene.getErsCorrection(), // final GeometryCorrection  geometryCorrection,
					disparity_corr,               // final double              disparity_corr,
					margin,                       // final int                 margin,      // do not use tiles if their centers are closer to the edges
					null,                         // final boolean []          valid_tiles,            
					QuadCLT.THREADS_MAX);                  // final int                 threadsMax)  // maximal number of threads to launch
			ref_scene.saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
			image_dtt.setReferenceTD( // tp_tasks_ref will be updated
					erase_clt,
					null,                       // final int []              wh,               // null (use sensor dimensions) or pair {width, height} in pixels
					clt_parameters.img_dtt,     // final ImageDttParameters  imgdtt_params,    // Now just extra correlation parameters, later will include, most others
					true, // final boolean             use_reference_buffer,
					tp_tasks_ref[0],               // final TpTask[]            tp_tasks,
					clt_parameters.gpu_sigma_r, // final double              gpu_sigma_r,     // 0.9, 1.1
					clt_parameters.gpu_sigma_b, // final double              gpu_sigma_b,     // 0.9, 1.1
					clt_parameters.gpu_sigma_g, // final double              gpu_sigma_g,     // 0.6, 0.7
					clt_parameters.gpu_sigma_m, // final double              gpu_sigma_m,     //  =       0.4; // 0.7;
					QuadCLT.THREADS_MAX,                 // final int                 threadsMax,       // maximal number of threads to launch
					debug_level);               // final int                 globalDebugLevel);
		}

		ref_scene.saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
		return tp_tasks_ref;
    }
	
    public static void generateEgomotionTable0(
			CLTParameters  clt_parameters,
			QuadCLT []     quadCLTs,
			int            ref_index,
			int            earliest_scene,
			String         path,
			String         comment) {
		boolean use_euler = true;
		boolean use_qn2b = true;
		double [] ims_ortho =     clt_parameters.imp.ims_ortho;
		double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
//    	double [] quat_ortho = {0.5, 0.5, -0.5, 0.5}; // approximate IMU orientation
		String header_img="#\ttimestamp\tx(m)\ty(m)\tz(m)\ta(rad)\ttilt(rad)\troll(rad)"+
			"\tVx(m/s)\tVy(m/s)\tVz(m/s)\tVa(rad/s)\tVt(rad/s)\tVr(rad/s)";
		String header_ins1="\ttow\ttheta0\ttheta1\ttheta2\tu(m/s)\tv(m/s)\tw(m/s)"+
			"\tlat\tlong\talt\tned0\tned1\tned2";
		String header_ins2="\ttow\tqn2b0\tqn2b1\tqn2b2\tqn2b3\tu(m/s)\tv(m/s)\tw(m/s)\tlat\tlong\talt";
		String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+
				"\tcam_X1\tcam_Y1\tcam_Z1\tcam_X2\tcam_Y2\tcam_Z2"+
				"\tabs_A\tabs_T\tabs_R\trel_A\trel_T\trel_R"+
				"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
		
		String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
		
		String header_cam000= "\ttheta0-000\ttheta1-000\ttheta2-000";
		String header_cam001= "\ttheta0-001\ttheta1-001\ttheta2-001";
		String header_cam010= "\ttheta0-010\ttheta1-010\ttheta2-010";
		String header_cam011= "\ttheta0-011\ttheta1-011\ttheta2-011";
		String header_cam100= "\ttheta0-100\ttheta1-100\ttheta2-100";
		String header_cam101= "\ttheta0-101\ttheta1-101\ttheta2-101";
		String header_cam110= "\ttheta0-110\ttheta1-110\ttheta2-110";
		String header_cam111= "\ttheta0-111\ttheta1-111\ttheta2-111";
		
		String header_cam200= "\ttheta0-200\ttheta1-200\ttheta2-200";
		String header_cam201= "\ttheta0-201\ttheta1-201\ttheta2-201";
		String header_cam210= "\ttheta0-210\ttheta1-210\ttheta2-210";
		String header_cam211= "\ttheta0-211\ttheta1-211\ttheta2-211";
		String header_cam300= "\ttheta0-300\ttheta1-300\ttheta2-300";
		String header_cam301= "\ttheta0-301\ttheta1-301\ttheta2-301";
		String header_cam310= "\ttheta0-310\ttheta1-310\ttheta2-310";
		String header_cam311= "\ttheta0-311\ttheta1-311\ttheta2-311";
		
		String header_cam400= "\ttheta0-400\ttheta1-400\ttheta2-400";
		String header_cam401= "\ttheta0-401\ttheta1-401\ttheta2-401";
		String header_cam410= "\ttheta0-410\ttheta1-410\ttheta2-410";
		String header_cam411= "\ttheta0-411\ttheta1-411\ttheta2-411";
		String header_cam500= "\ttheta0-500\ttheta1-500\ttheta2-500";
		String header_cam501= "\ttheta0-501\ttheta1-501\ttheta2-501";
		String header_cam510= "\ttheta0-510\ttheta1-510\ttheta2-510";
		String header_cam511= "\ttheta0-511\ttheta1-511\ttheta2-511";
		
		String header_qn2b000= "\tqn2b0-000\tqn2b1-000\tqn2b2-000";
		String header_qn2b001= "\tqn2b0-001\tqn2b1-001\tqn2b2-001";
		String header_qn2b010= "\tqn2b0-010\tqn2b1-010\tqn2b2-010";
		String header_qn2b011= "\tqn2b0-011\tqn2b1-011\tqn2b2-011";
		String header_qn2b100= "\tqn2b0-100\tqn2b1-100\tqn2b2-100";
		String header_qn2b101= "\tqn2b0-101\tqn2b1-101\tqn2b2-101";
		String header_qn2b110= "\tqn2b0-110\tqn2b1-110\tqn2b2-110";
		String header_qn2b111= "\tqn2b0-111\tqn2b1-111\tqn2b2-111";

		String header_qn2b200= "\tqn2b0-200\tqn2b1-200\tqn2b2-200";
		String header_qn2b201= "\tqn2b0-201\tqn2b1-201\tqn2b2-201";
		String header_qn2b210= "\tqn2b0-210\tqn2b1-210\tqn2b2-210";
		String header_qn2b211= "\tqn2b0-211\tqn2b1-211\tqn2b2-211";
		String header_qn2b300= "\tqn2b0-300\tqn2b1-300\tqn2b2-300";
		String header_qn2b301= "\tqn2b0-301\tqn2b1-301\tqn2b2-301";
		String header_qn2b310= "\tqn2b0-310\tqn2b1-310\tqn2b2-310";
		String header_qn2b311= "\tqn2b0-311\tqn2b1-311\tqn2b2-311";

		String header_qn2b400= "\tqn2b0-400\tqn2b1-400\tqn2b2-400";
		String header_qn2b401= "\tqn2b0-401\tqn2b1-401\tqn2b2-401";
		String header_qn2b410= "\tqn2b0-410\tqn2b1-410\tqn2b2-410";
		String header_qn2b411= "\tqn2b0-411\tqn2b1-411\tqn2b2-411";
		String header_qn2b500= "\tqn2b0-500\tqn2b1-500\tqn2b2-500";
		String header_qn2b501= "\tqn2b0-501\tqn2b1-501\tqn2b2-501";
		String header_qn2b510= "\tqn2b0-510\tqn2b1-510\tqn2b2-510";
		String header_qn2b511= "\tqn2b0-511\tqn2b1-511\tqn2b2-511";
		
		String header = header_img+header_ins1+header_ins2+header_ins2_extra+header_pimu;
		if (use_qn2b) {
			header+=
					header_qn2b000 + header_qn2b001 + header_qn2b010 + header_qn2b011 +
					header_qn2b100 + header_qn2b101 + header_qn2b110 + header_qn2b111 +
					header_qn2b200 + header_qn2b201 + header_qn2b210 + header_qn2b211 +
					header_qn2b300 + header_qn2b301 + header_qn2b310 + header_qn2b311 +
					header_qn2b400 + header_qn2b401 + header_qn2b410 + header_qn2b411 +
					header_qn2b500 + header_qn2b501 + header_qn2b510 + header_qn2b511;
		}
		if (use_euler) {
			header+=
				header_cam000 + header_cam001 + header_cam010 + header_cam011 +
				header_cam100 + header_cam101 + header_cam110 + header_cam111 +
				header_cam200 + header_cam201 + header_cam210 + header_cam211 +
				header_cam300 + header_cam301 + header_cam310 + header_cam311 +
				header_cam400 + header_cam401 + header_cam410 + header_cam411 +
				header_cam500 + header_cam501 + header_cam510 + header_cam511;
		}
		StringBuffer sb = new StringBuffer();
		QuadCLT ref_scene = quadCLTs[ref_index];
		ErsCorrection ers_reference = ref_scene.getErsCorrection();
		double [][][] scenes_xyzatr = new double [quadCLTs.length][][];
		for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
			QuadCLT scene = quadCLTs[nscene];
			if (nscene == ref_index) {
				scenes_xyzatr[nscene] = new double[2][3];
			} else {
				String ts = scene.getImageName();
				scenes_xyzatr[nscene] = new double [][] {ers_reference.getSceneXYZ(ts),ers_reference.getSceneATR(ts)};
			}
		}
		double [][][] dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
        		quadCLTs,          // QuadCLT []     scenes, // ordered by increasing timestamps
        		ref_index,
        		earliest_scene,    // int            start_scene,
        		quadCLTs.length-1, // int            end1_scene,
        		scenes_xyzatr,     // double [][][]  scenes_xyzatr,
        		clt_parameters.ofp.lpf_series); // half_run_range); // double         half_run_range
		Did_ins_2   d2_ref = quadCLTs[ref_index].did_ins_2;
		double [] cam_quat_ref =Imx5.quaternionImsToCam(d2_ref.getQn2b() ,
				ims_mount_atr, // new double[] {0, 0.13, 0},
				ims_ortho);
		double [] ref_abs_atr = Imx5.quatToCamAtr(cam_quat_ref);
		double [][] ims_ref_xyzatr = {ZERO3, ref_abs_atr};
		
		for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
			QuadCLT scene = quadCLTs[nscene];
			if (scene == null) {
				for (int i = 0; i < 26; i++) {
					sb.append("\t---");
				}
				sb.append("\n");
				continue;
			}
			double timestamp = scene.getTimeStamp();
			Did_ins_1   d1 = scene.did_ins_1;
			Did_ins_2   d2 = scene.did_ins_2;
			Did_pimu    d3 = scene.did_pimu;
			
			sb.append(nscene+"\t"+timestamp);
			// TODO: try saved, not calculated velocities!
			sb.append("\t"+scenes_xyzatr[nscene][0][0]+"\t"+scenes_xyzatr[nscene][0][1]+"\t"+scenes_xyzatr[nscene][0][2]);
			sb.append("\t"+scenes_xyzatr[nscene][1][0]+"\t"+scenes_xyzatr[nscene][1][1]+"\t"+scenes_xyzatr[nscene][1][2]);
			
			sb.append("\t"+dxyzatr_dt[nscene][0][0]+"\t"+dxyzatr_dt[nscene][0][1]+"\t"+dxyzatr_dt[nscene][0][2]);
			sb.append("\t"+dxyzatr_dt[nscene][1][0]+"\t"+dxyzatr_dt[nscene][1][1]+"\t"+dxyzatr_dt[nscene][1][2]);
			
//			String header_ins1="\ttow\ttheta0\ttheta1\ttheta2\tu(m/s)\tv(m/s)\tw(m/s)"+
//					"\tlong\tlat\talt\tned0\tned1\tned2";
			sb.append("\t"+d1.timeOfWeek);
			sb.append("\t"+d1.theta[0]+"\t"+d1.theta[1]+"\t"+d1.theta[2]);
			sb.append("\t"+d1.uvw[0]+  "\t"+d1.uvw[1]+  "\t"+d1.uvw[2]);
			sb.append("\t"+d1.lla[0]+  "\t"+d1.lla[1]+  "\t"+d1.lla[2]);
			sb.append("\t"+d1.ned[0]+  "\t"+d1.ned[1]+  "\t"+d1.ned[2]);
//String header_ins2="\ttow\tqn2b0\tqn2b1\tqn2b2\tqn2b3\tu(m/s)\tv(m/s)\tw(m/s)\tlat\tlong\talt";
			sb.append("\t"+d2.timeOfWeek);
			sb.append("\t"+d2.qn2b[0]+"\t"+d2.qn2b[1]+"\t"+d2.qn2b[2]+"\t"+d2.qn2b[3]);
			sb.append("\t"+d2.uvw[0]+  "\t"+d2.uvw[1]+  "\t"+d2.uvw[2]);
			sb.append("\t"+d2.lla[0]+  "\t"+d2.lla[1]+  "\t"+d2.lla[2]);
			double [] double_theta = d1.getTheta();
//imsToCamRotations(double [] ims_theta, boolean rev_order, boolean rev_matrix )
			double [] double_qn2b = d2.getQn2b();
			double [] double_uvw =  d2.getUvw();
			double [] uvw_dir = Imx5.applyQuternionTo(double_qn2b, double_uvw, false); // bad
			double [] uvw_inv = Imx5.applyQuternionTo(double_qn2b, double_uvw, true); // good
//Converting from local uvw to NED: (qn2b).applyInverseTo(uvw,ned) results in [vNorth, vEast, vUp] 			
			
			double [] ned = Imx5.nedFromLla (d2.lla, d2_ref.lla);			
			double [] ims_xyz = Imx5.applyQuternionTo(double_qn2b, ned, false);
//			String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+
//					"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
			
			sb.append("\t"+ned[0]+  "\t"+ned[1]+  "\t"+ned[2]);             // global axes
			sb.append("\t"+ims_xyz[0]+  "\t"+ims_xyz[1]+  "\t"+ims_xyz[2]); // imu axes
			
			double [] cam_quat1 =Imx5.quaternionImsToCam(double_qn2b, new double[3], ims_ortho);
			double [] cam_quat2 =Imx5.quaternionImsToCam(double_qn2b,
					ims_mount_atr, // new double[] {0, 0.13, 0},
					ims_ortho);
			double [] cam_xyz1 = Imx5.applyQuternionTo(cam_quat1, ned, false);
			double [] cam_xyz2 = Imx5.applyQuternionTo(cam_quat2, ned, false);
			
			sb.append("\t"+cam_xyz1[0]+  "\t"+cam_xyz1[1]+  "\t"+cam_xyz1[2]); // 
			sb.append("\t"+cam_xyz2[0]+  "\t"+cam_xyz2[1]+  "\t"+cam_xyz2[2]); //
			
			double [] scene_abs_atr = Imx5.quatToCamAtr(cam_quat2);
			double [][] ims_scene_xyzatr = {ZERO3, scene_abs_atr};
			double [] scene_rel_atr=ErsCorrection.combineXYZATR(
					ims_scene_xyzatr,
					ErsCorrection.invertXYZATR(ims_ref_xyzatr))[1];
			
			sb.append("\t"+scene_abs_atr[0]+  "\t"+scene_abs_atr[1]+  "\t"+scene_abs_atr[2]); // 
			sb.append("\t"+scene_rel_atr[0]+  "\t"+scene_rel_atr[1]+  "\t"+scene_rel_atr[2]); //
			
			
			sb.append("\t"+uvw_dir[0]+  "\t"+uvw_dir[1]+  "\t"+uvw_dir[2]); // wrong
			sb.append("\t"+uvw_inv[0]+  "\t"+uvw_inv[1]+  "\t"+uvw_inv[2]); // correct
			
//				String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
			sb.append("\t"+(d3.theta[0]/d3.dt)+"\t"+(d3.theta[1]/d3.dt)+"\t"+(d3.theta[2]/d3.dt));
			sb.append("\t"+(d3.vel[0]/d3.dt)+"\t"+(d3.vel[1]/d3.dt)+"\t"+(d3.vel[2]/d3.dt));
// 		String header_cam= "\ttheta0\ttheta1\ttheta2\troll\tpitch\tyaw";
			if (use_qn2b) {
				double [] cam_qn2b_000 = Imx5.imsQToCamRotations(double_qn2b, 0, false, false);		
				double [] cam_qn2b_001 = Imx5.imsQToCamRotations(double_qn2b, 0, false, true);		
				double [] cam_qn2b_010 = Imx5.imsQToCamRotations(double_qn2b, 0, true,  false);		
				double [] cam_qn2b_011 = Imx5.imsQToCamRotations(double_qn2b, 0, true,  true);		
				double [] cam_qn2b_100 = Imx5.imsQToCamRotations(double_qn2b, 1, false, false);		
				double [] cam_qn2b_101 = Imx5.imsQToCamRotations(double_qn2b, 1, false, true);		
				double [] cam_qn2b_110 = Imx5.imsQToCamRotations(double_qn2b, 1, true,  false);		
				double [] cam_qn2b_111 = Imx5.imsQToCamRotations(double_qn2b, 1, true,  true);

				double [] cam_qn2b_200 = Imx5.imsQToCamRotations(double_qn2b, 2, false, false);		
				double [] cam_qn2b_201 = Imx5.imsQToCamRotations(double_qn2b, 2, false, true);		
				double [] cam_qn2b_210 = Imx5.imsQToCamRotations(double_qn2b, 2, true,  false);		
				double [] cam_qn2b_211 = Imx5.imsQToCamRotations(double_qn2b, 2, true,  true);		
				double [] cam_qn2b_300 = Imx5.imsQToCamRotations(double_qn2b, 3, false, false);		
				double [] cam_qn2b_301 = Imx5.imsQToCamRotations(double_qn2b, 3, false, true);		
				double [] cam_qn2b_310 = Imx5.imsQToCamRotations(double_qn2b, 3, true,  false);		
				double [] cam_qn2b_311 = Imx5.imsQToCamRotations(double_qn2b, 3, true,  true);

				double [] cam_qn2b_400 = Imx5.imsQToCamRotations(double_qn2b, 4, false, false);		
				double [] cam_qn2b_401 = Imx5.imsQToCamRotations(double_qn2b, 4, false, true);		
				double [] cam_qn2b_410 = Imx5.imsQToCamRotations(double_qn2b, 4, true,  false);		
				double [] cam_qn2b_411 = Imx5.imsQToCamRotations(double_qn2b, 4, true,  true);		
				double [] cam_qn2b_500 = Imx5.imsQToCamRotations(double_qn2b, 5, false, false);		
				double [] cam_qn2b_501 = Imx5.imsQToCamRotations(double_qn2b, 5, false, true);		
				double [] cam_qn2b_510 = Imx5.imsQToCamRotations(double_qn2b, 5, true,  false);		
				double [] cam_qn2b_511 = Imx5.imsQToCamRotations(double_qn2b, 5, true,  true);

				sb.append("\t"+cam_qn2b_000[0]+"\t"+cam_qn2b_000[1]+"\t"+cam_qn2b_000[2]);
				sb.append("\t"+cam_qn2b_001[0]+"\t"+cam_qn2b_001[1]+"\t"+cam_qn2b_001[2]);
				sb.append("\t"+cam_qn2b_010[0]+"\t"+cam_qn2b_010[1]+"\t"+cam_qn2b_010[2]);
				sb.append("\t"+cam_qn2b_011[0]+"\t"+cam_qn2b_011[1]+"\t"+cam_qn2b_011[2]);
				sb.append("\t"+cam_qn2b_100[0]+"\t"+cam_qn2b_100[1]+"\t"+cam_qn2b_100[2]);
				sb.append("\t"+cam_qn2b_101[0]+"\t"+cam_qn2b_101[1]+"\t"+cam_qn2b_101[2]);
				sb.append("\t"+cam_qn2b_110[0]+"\t"+cam_qn2b_110[1]+"\t"+cam_qn2b_110[2]);
				sb.append("\t"+cam_qn2b_111[0]+"\t"+cam_qn2b_111[1]+"\t"+cam_qn2b_111[2]);

				sb.append("\t"+cam_qn2b_200[0]+"\t"+cam_qn2b_200[1]+"\t"+cam_qn2b_200[2]);
				sb.append("\t"+cam_qn2b_201[0]+"\t"+cam_qn2b_201[1]+"\t"+cam_qn2b_201[2]);
				sb.append("\t"+cam_qn2b_210[0]+"\t"+cam_qn2b_210[1]+"\t"+cam_qn2b_210[2]);
				sb.append("\t"+cam_qn2b_211[0]+"\t"+cam_qn2b_211[1]+"\t"+cam_qn2b_211[2]);
				sb.append("\t"+cam_qn2b_300[0]+"\t"+cam_qn2b_300[1]+"\t"+cam_qn2b_300[2]);
				sb.append("\t"+cam_qn2b_301[0]+"\t"+cam_qn2b_301[1]+"\t"+cam_qn2b_301[2]);
				sb.append("\t"+cam_qn2b_310[0]+"\t"+cam_qn2b_310[1]+"\t"+cam_qn2b_310[2]);
				sb.append("\t"+cam_qn2b_311[0]+"\t"+cam_qn2b_311[1]+"\t"+cam_qn2b_311[2]);

				sb.append("\t"+cam_qn2b_400[0]+"\t"+cam_qn2b_400[1]+"\t"+cam_qn2b_400[2]);
				sb.append("\t"+cam_qn2b_401[0]+"\t"+cam_qn2b_401[1]+"\t"+cam_qn2b_401[2]);
				sb.append("\t"+cam_qn2b_410[0]+"\t"+cam_qn2b_410[1]+"\t"+cam_qn2b_410[2]);
				sb.append("\t"+cam_qn2b_411[0]+"\t"+cam_qn2b_411[1]+"\t"+cam_qn2b_411[2]);
				sb.append("\t"+cam_qn2b_500[0]+"\t"+cam_qn2b_500[1]+"\t"+cam_qn2b_500[2]);
				sb.append("\t"+cam_qn2b_501[0]+"\t"+cam_qn2b_501[1]+"\t"+cam_qn2b_501[2]);
				sb.append("\t"+cam_qn2b_510[0]+"\t"+cam_qn2b_510[1]+"\t"+cam_qn2b_510[2]);
				sb.append("\t"+cam_qn2b_511[0]+"\t"+cam_qn2b_511[1]+"\t"+cam_qn2b_511[2]);
			}

			if (use_euler) {
				double [] cam_theta_000 = Imx5.imsToCamRotations(double_theta, 0, false, false);		
				double [] cam_theta_001 = Imx5.imsToCamRotations(double_theta, 0, false, true);		
				double [] cam_theta_010 = Imx5.imsToCamRotations(double_theta, 0, true,  false);		
				double [] cam_theta_011 = Imx5.imsToCamRotations(double_theta, 0, true,  true);		
				double [] cam_theta_100 = Imx5.imsToCamRotations(double_theta, 1, false, false);		
				double [] cam_theta_101 = Imx5.imsToCamRotations(double_theta, 1, false, true);		
				double [] cam_theta_110 = Imx5.imsToCamRotations(double_theta, 1, true,  false);		
				double [] cam_theta_111 = Imx5.imsToCamRotations(double_theta, 1, true,  true);

				double [] cam_theta_200 = Imx5.imsToCamRotations(double_theta, 2, false, false);		
				double [] cam_theta_201 = Imx5.imsToCamRotations(double_theta, 2, false, true);		
				double [] cam_theta_210 = Imx5.imsToCamRotations(double_theta, 2, true,  false);		
				double [] cam_theta_211 = Imx5.imsToCamRotations(double_theta, 2, true,  true);		
				double [] cam_theta_300 = Imx5.imsToCamRotations(double_theta, 3, false, false);		
				double [] cam_theta_301 = Imx5.imsToCamRotations(double_theta, 3, false, true);		
				double [] cam_theta_310 = Imx5.imsToCamRotations(double_theta, 3, true,  false);		
				double [] cam_theta_311 = Imx5.imsToCamRotations(double_theta, 3, true,  true);

				double [] cam_theta_400 = Imx5.imsToCamRotations(double_theta, 4, false, false);		
				double [] cam_theta_401 = Imx5.imsToCamRotations(double_theta, 4, false, true);		
				double [] cam_theta_410 = Imx5.imsToCamRotations(double_theta, 4, true,  false);		
				double [] cam_theta_411 = Imx5.imsToCamRotations(double_theta, 4, true,  true);		
				double [] cam_theta_500 = Imx5.imsToCamRotations(double_theta, 5, false, false);		
				double [] cam_theta_501 = Imx5.imsToCamRotations(double_theta, 5, false, true);		
				double [] cam_theta_510 = Imx5.imsToCamRotations(double_theta, 5, true,  false);		
				double [] cam_theta_511 = Imx5.imsToCamRotations(double_theta, 5, true,  true);

				sb.append("\t"+cam_theta_000[0]+"\t"+cam_theta_000[1]+"\t"+cam_theta_000[2]);
				sb.append("\t"+cam_theta_001[0]+"\t"+cam_theta_001[1]+"\t"+cam_theta_001[2]);
				sb.append("\t"+cam_theta_010[0]+"\t"+cam_theta_010[1]+"\t"+cam_theta_010[2]);
				sb.append("\t"+cam_theta_011[0]+"\t"+cam_theta_011[1]+"\t"+cam_theta_011[2]);
				sb.append("\t"+cam_theta_100[0]+"\t"+cam_theta_100[1]+"\t"+cam_theta_100[2]);
				sb.append("\t"+cam_theta_101[0]+"\t"+cam_theta_101[1]+"\t"+cam_theta_101[2]);
				sb.append("\t"+cam_theta_110[0]+"\t"+cam_theta_110[1]+"\t"+cam_theta_110[2]);
				sb.append("\t"+cam_theta_111[0]+"\t"+cam_theta_111[1]+"\t"+cam_theta_111[2]);

				sb.append("\t"+cam_theta_200[0]+"\t"+cam_theta_200[1]+"\t"+cam_theta_200[2]);
				sb.append("\t"+cam_theta_201[0]+"\t"+cam_theta_201[1]+"\t"+cam_theta_201[2]);
				sb.append("\t"+cam_theta_210[0]+"\t"+cam_theta_210[1]+"\t"+cam_theta_210[2]);
				sb.append("\t"+cam_theta_211[0]+"\t"+cam_theta_211[1]+"\t"+cam_theta_211[2]);
				sb.append("\t"+cam_theta_300[0]+"\t"+cam_theta_300[1]+"\t"+cam_theta_300[2]);
				sb.append("\t"+cam_theta_301[0]+"\t"+cam_theta_301[1]+"\t"+cam_theta_301[2]);
				sb.append("\t"+cam_theta_310[0]+"\t"+cam_theta_310[1]+"\t"+cam_theta_310[2]);
				sb.append("\t"+cam_theta_311[0]+"\t"+cam_theta_311[1]+"\t"+cam_theta_311[2]);

				sb.append("\t"+cam_theta_400[0]+"\t"+cam_theta_400[1]+"\t"+cam_theta_400[2]);
				sb.append("\t"+cam_theta_401[0]+"\t"+cam_theta_401[1]+"\t"+cam_theta_401[2]);
				sb.append("\t"+cam_theta_410[0]+"\t"+cam_theta_410[1]+"\t"+cam_theta_410[2]);
				sb.append("\t"+cam_theta_411[0]+"\t"+cam_theta_411[1]+"\t"+cam_theta_411[2]);
				sb.append("\t"+cam_theta_500[0]+"\t"+cam_theta_500[1]+"\t"+cam_theta_500[2]);
				sb.append("\t"+cam_theta_501[0]+"\t"+cam_theta_501[1]+"\t"+cam_theta_501[2]);
				sb.append("\t"+cam_theta_510[0]+"\t"+cam_theta_510[1]+"\t"+cam_theta_510[2]);
				sb.append("\t"+cam_theta_511[0]+"\t"+cam_theta_511[1]+"\t"+cam_theta_511[2]);
			}
			/*
			sb.append("\t"+
					cam_theta[0]*180/Math.PI+"\t"+
					cam_theta[1]*180/Math.PI+"\t"+
					cam_theta[2]*180/Math.PI);
			*/
			sb.append("\n");
		}
		
		if (path!=null) {
			String footer=(comment != null) ? ("Comment: "+comment): "";
			CalibrationFileManagement.saveStringToFile (
					path,
					header+"\n"+sb.toString()+"\n"+footer);
		}else{
			new TextWindow("Sharpness History", header, sb.toString(), 1000,900);
		}
		
    	
    }

    public static void generateEgomotionTable(
			CLTParameters  clt_parameters,
			QuadCLT []     quadCLTs,
			int            ref_index,
			int            earliest_scene,
			String         path,
			String         comment) {
		double [] ims_ortho =     clt_parameters.imp.ims_ortho;
		double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
//    	double [] quat_ortho = {0.5, 0.5, -0.5, 0.5}; // approximate IMU orientation
		String header_img="#\ttimestamp\tx(m)\ty(m)\tz(m)\ta(rad)\ttilt(rad)\troll(rad)"+
			"\tVx(m/s)\tVy(m/s)\tVz(m/s)\tVa(rad/s)\tVt(rad/s)\tVr(rad/s)";
		String header_ins1="\ttow\ttheta0\ttheta1\ttheta2\tu(m/s)\tv(m/s)\tw(m/s)"+
			"\tlat\tlong\talt\tned0\tned1\tned2";
		String header_ins2="\ttow\tqn2b0\tqn2b1\tqn2b2\tqn2b3\tu(m/s)\tv(m/s)\tw(m/s)\tlat\tlong\talt";
		String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+
				"\tcam_X1\tcam_Y1\tcam_Z1\tcam_X2\tcam_Y2\tcam_Z2"+
				"\tned->X\tned->Y\tned->Z\tenu->X2\tenu->Y\tenu->Z"+
				"\tabs_A_ned\tabs_T_ned\tabs_R_ned\trel_A_ned\trel_T_ned\trel_R_ned"+
				"\tabs_A_enu\tabs_T_enu\tabs_R_enu\trel_A_enu\trel_T_enu\trel_R_enu"+
				"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
		
		String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
		
		
		String header = header_img+header_ins1+header_ins2+header_ins2_extra+header_pimu;
		StringBuffer sb = new StringBuffer();
		QuadCLT ref_scene = quadCLTs[ref_index];
		ErsCorrection ers_reference = ref_scene.getErsCorrection();
		double [][][] scenes_xyzatr = new double [quadCLTs.length][][];
		for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
			QuadCLT scene = quadCLTs[nscene];
			if (nscene == ref_index) {
				scenes_xyzatr[nscene] = new double[2][3];
			} else {
				String ts = scene.getImageName();
				scenes_xyzatr[nscene] = new double [][] {ers_reference.getSceneXYZ(ts),ers_reference.getSceneATR(ts)};
			}
		}
		double [][][] dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
        		quadCLTs,          // QuadCLT []     scenes, // ordered by increasing timestamps
        		ref_index,
        		earliest_scene,    // int            start_scene,
        		quadCLTs.length-1, // int            end1_scene,
        		scenes_xyzatr,     // double [][][]  scenes_xyzatr,
        		clt_parameters.ofp.lpf_series); // half_run_range); // double         half_run_range
		Did_ins_2   d2_ref = quadCLTs[ref_index].did_ins_2;
		double [] cam_quat_ref =Imx5.quaternionImsToCam(d2_ref.getQn2b() ,
				ims_mount_atr, // new double[] {0, 0.13, 0},
				ims_ortho);
		double [] ref_abs_atr = Imx5.quatToCamAtr(cam_quat_ref);
		double [][] ims_ref_xyzatr = {ZERO3, ref_abs_atr};
		
		
		double [] cam_quat_ref_enu =Imx5.quaternionImsToCam(d2_ref.getQEnu() ,
				ims_mount_atr, // new double[] {0, 0.13, 0},
				ims_ortho);
		double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu);
		double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu};
		
		
		for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
			QuadCLT scene = quadCLTs[nscene];
			if (scene == null) {
				for (int i = 0; i < 26; i++) {
					sb.append("\t---");
				}
				sb.append("\n");
				continue;
			}
			double timestamp = scene.getTimeStamp();
			Did_ins_1   d1 = scene.did_ins_1;
			Did_ins_2   d2 = scene.did_ins_2;
			Did_pimu    d3 = scene.did_pimu;
			
			sb.append(nscene+"\t"+timestamp);
			// TODO: try saved, not calculated velocities!
			sb.append("\t"+scenes_xyzatr[nscene][0][0]+"\t"+scenes_xyzatr[nscene][0][1]+"\t"+scenes_xyzatr[nscene][0][2]);
			sb.append("\t"+scenes_xyzatr[nscene][1][0]+"\t"+scenes_xyzatr[nscene][1][1]+"\t"+scenes_xyzatr[nscene][1][2]);
			
			sb.append("\t"+dxyzatr_dt[nscene][0][0]+"\t"+dxyzatr_dt[nscene][0][1]+"\t"+dxyzatr_dt[nscene][0][2]);
			sb.append("\t"+dxyzatr_dt[nscene][1][0]+"\t"+dxyzatr_dt[nscene][1][1]+"\t"+dxyzatr_dt[nscene][1][2]);
			
//			String header_ins1="\ttow\ttheta0\ttheta1\ttheta2\tu(m/s)\tv(m/s)\tw(m/s)"+
//					"\tlong\tlat\talt\tned0\tned1\tned2";
			sb.append("\t"+d1.timeOfWeek);
			sb.append("\t"+d1.theta[0]+"\t"+d1.theta[1]+"\t"+d1.theta[2]);
			sb.append("\t"+d1.uvw[0]+  "\t"+d1.uvw[1]+  "\t"+d1.uvw[2]);
			sb.append("\t"+d1.lla[0]+  "\t"+d1.lla[1]+  "\t"+d1.lla[2]);
			sb.append("\t"+d1.ned[0]+  "\t"+d1.ned[1]+  "\t"+d1.ned[2]);
//String header_ins2="\ttow\tqn2b0\tqn2b1\tqn2b2\tqn2b3\tu(m/s)\tv(m/s)\tw(m/s)\tlat\tlong\talt";
			sb.append("\t"+d2.timeOfWeek);
			sb.append("\t"+d2.qn2b[0]+"\t"+d2.qn2b[1]+"\t"+d2.qn2b[2]+"\t"+d2.qn2b[3]);
			sb.append("\t"+d2.uvw[0]+  "\t"+d2.uvw[1]+  "\t"+d2.uvw[2]);
			sb.append("\t"+d2.lla[0]+  "\t"+d2.lla[1]+  "\t"+d2.lla[2]);
			double [] double_theta = d1.getTheta();
//imsToCamRotations(double [] ims_theta, boolean rev_order, boolean rev_matrix )
			double [] double_qn2b = d2.getQn2b();
			double [] double_uvw =  d2.getUvw();
			double [] uvw_dir = Imx5.applyQuternionTo(double_qn2b, double_uvw, false); // bad
			double [] uvw_inv = Imx5.applyQuternionTo(double_qn2b, double_uvw, true); // good
//Converting from local uvw to NED: (qn2b).applyInverseTo(uvw,ned) results in [vNorth, vEast, vUp] 			
			
			double [] ned = Imx5.nedFromLla (d2.lla, d2_ref.lla);
			double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla);
			double [] ims_xyz = Imx5.applyQuternionTo(double_qn2b, ned, false);
//			String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+
//					"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
			
			sb.append("\t"+ned[0]+  "\t"+ned[1]+  "\t"+ned[2]);             // global axes
			sb.append("\t"+ims_xyz[0]+  "\t"+ims_xyz[1]+  "\t"+ims_xyz[2]); // imu axes
			
			double [] cam_quat1 =Imx5.quaternionImsToCam(double_qn2b, new double[3], ims_ortho);
			double [] cam_quat2 =Imx5.quaternionImsToCam(double_qn2b,
					ims_mount_atr, // new double[] {0, 0.13, 0},
					ims_ortho);
			double [] cam_quat_enu =Imx5.quaternionImsToCam(d2.getQEnu(),
					ims_mount_atr, // new double[] {0, 0.13, 0},
					ims_ortho);
			
			double [] cam_xyz1 = Imx5.applyQuternionTo(cam_quat1, ned, false);
			double [] cam_xyz2 = Imx5.applyQuternionTo(cam_quat2, ned, false);
			double [] cam_xyz_ned = test_xyz_ned(
					Imx5.nedFromLla (d2.lla, d2_ref.lla), // double [] ned,double [] ned,
					d2.getQn2b(), // double[]  quat_ned,
					ims_mount_atr, // double [] ims_mount_atr,
					ims_ortho); //double [] ims_ortho)
			double [] cam_xyz_enu = test_xyz_enu(
					Imx5.enuFromLla (d2.lla, d2_ref.lla), //double [] enu,
					d2.getQEnu(), // double[]  quat_enu,
					ims_mount_atr, // double [] ims_mount_atr,
					ims_ortho); //double [] ims_ortho)
			
			sb.append("\t"+cam_xyz1[0]+  "\t"+cam_xyz1[1]+  "\t"+cam_xyz1[2]); // 
			sb.append("\t"+cam_xyz2[0]+  "\t"+cam_xyz2[1]+  "\t"+cam_xyz2[2]); //

			sb.append("\t"+cam_xyz_ned[0]+  "\t"+cam_xyz_ned[1]+  "\t"+cam_xyz_ned[2]); // 
			sb.append("\t"+cam_xyz_enu[0]+  "\t"+cam_xyz_enu[1]+  "\t"+cam_xyz_enu[2]); //
			
			double [] scene_abs_atr = Imx5.quatToCamAtr(cam_quat2);
			
			double [][] ims_scene_xyzatr = {ZERO3, scene_abs_atr};
			double [] scene_rel_atr=ErsCorrection.combineXYZATR(
					ims_scene_xyzatr,
					ErsCorrection.invertXYZATR(ims_ref_xyzatr))[1];
			
			double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu);
			double [][] ims_scene_xyzatr_enu = {ZERO3, scene_abs_atr_enu};

			double [] scene_rel_atr_enu=ErsCorrection.combineXYZATR(
					ims_scene_xyzatr_enu,
					ErsCorrection.invertXYZATR(ims_ref_xyzatr_enu))[1];
			
			sb.append("\t"+scene_abs_atr[0]+  "\t"+scene_abs_atr[1]+  "\t"+scene_abs_atr[2]); // 
			sb.append("\t"+scene_rel_atr[0]+  "\t"+scene_rel_atr[1]+  "\t"+scene_rel_atr[2]); //
			
			sb.append("\t"+scene_abs_atr_enu[0]+  "\t"+scene_abs_atr_enu[1]+  "\t"+scene_abs_atr_enu[2]); // 
			sb.append("\t"+scene_rel_atr_enu[0]+  "\t"+scene_rel_atr_enu[1]+  "\t"+scene_rel_atr_enu[2]); //
			
			sb.append("\t"+uvw_dir[0]+  "\t"+uvw_dir[1]+  "\t"+uvw_dir[2]); // wrong
			sb.append("\t"+uvw_inv[0]+  "\t"+uvw_inv[1]+  "\t"+uvw_inv[2]); // correct
			
//				String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
			sb.append("\t"+(d3.theta[0]/d3.dt)+"\t"+(d3.theta[1]/d3.dt)+"\t"+(d3.theta[2]/d3.dt));
			sb.append("\t"+(d3.vel[0]/d3.dt)+"\t"+(d3.vel[1]/d3.dt)+"\t"+(d3.vel[2]/d3.dt));
// 		String header_cam= "\ttheta0\ttheta1\ttheta2\troll\tpitch\tyaw";
			/*
			sb.append("\t"+
					cam_theta[0]*180/Math.PI+"\t"+
					cam_theta[1]*180/Math.PI+"\t"+
					cam_theta[2]*180/Math.PI);
			*/
			sb.append("\n");
		}
		
		if (path!=null) {
			String footer=(comment != null) ? ("Comment: "+comment): "";
			CalibrationFileManagement.saveStringToFile (
					path,
					header+"\n"+sb.toString()+"\n"+footer);
		}else{
			new TextWindow("Sharpness History", header, sb.toString(), 1000,900);
		}
    }
    /*
    static double [] test_ned(
    		double [] ned,
    		double [] enu,
			double[]  quat_ned,
			double[]  quat_enu,
			double [] ims_mount_atr,
			double [] ims_ortho) {
//			double [] cam_xyzs = Imx5.applyQuternionTo(cam_quats, enu, false);
//    	double [] enu = new double [] {ned[1], ned[0], -ned[2]}; 
		
		double [] cam_quat2 =Imx5.quaternionImsToCam(quat,
				ims_mount_atr,
				ims_ortho);
		double [] cam_xyz2 = Imx5.applyQuternionTo(cam_quat2, ned, false);
		
		double [] cam_quat3 =Imx5.quaternionImsToCam(Imx5.quatEnu(quat_ned),
				ims_mount_atr,
				ims_ortho);
		double [] cam_xyz3 = Imx5.applyQuternionTo(cam_quat3, enu, false);
		return cam_xyz2;
    }
  */  
    static double [] test_xyz_ned(
    		double [] ned,
			double[]  quat_ned,
			double [] ims_mount_atr,
			double [] ims_ortho) {
		
		double [] cam_quat2 =Imx5.quaternionImsToCam(quat_ned,
				ims_mount_atr,
				ims_ortho);
		return Imx5.applyQuternionTo(cam_quat2, ned, false);
    }
      
    static double [] test_xyz_enu(
    		double [] enu,
			double[]  quat_enu,
			double [] ims_mount_atr,
			double [] ims_ortho) {
		
		double [] cam_quat2 =Imx5.quaternionImsToCam(quat_enu,
				ims_mount_atr,
				ims_ortho);
		return Imx5.applyQuternionTo(cam_quat2, enu, false);
    }
    
    
    static double [] test_ned0(
    		double [] ned,
			double[]  quat,
			double [] ims_mount_atr,
			double [] ims_ortho) {
		double [][] enu_ned = {{0,1,0},{1, 0, 0},{0, 0, -1}};
		Rotation rot_enu_ned = new Rotation(enu_ned, 1E-8);
//			double [] cam_xyzs = Imx5.applyQuternionTo(cam_quats, enu, false);
		Rotation quat_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
    	double [] enu = new double [] {ned[1], ned[0], -ned[2]}; 
		double [] rslt = new double[3]; // same as ned
		rot_enu_ned.applyTo(enu, rslt);
		
		double [] rslt1 = new double[3];
		double [] rslt2 = new double[3];
		quat_rot.applyTo(ned,rslt1);
//		Rotation qr = rot_enu_ned.applyTo(quat_rot);
		Rotation qr = quat_rot.applyTo(rot_enu_ned);
		qr.applyTo(enu,rslt2);
    	
		double [] cam_quat2 =Imx5.quaternionImsToCam(quat,
				ims_mount_atr,
				ims_ortho);
		double [] cam_xyz2 = Imx5.applyQuternionTo(cam_quat2, ned, false);
		
		double [][] reord = new double [16][4];
		for (int i = 0; i < reord.length; i++) {
			for (int j = 0; j<4; j++) {
				boolean neg = ((i >> j) & 1) > 0;
				reord[i][j] = 0.5 * (neg? -1 : 1);
			}
		}
		System.out.println ("cam_xyz2: "+
		cam_xyz2[0]+", "+ cam_xyz2[1]+", "+ cam_xyz2[2]);
		Rotation rot_reord;
		Rotation quat_enu_rot;
		double [] quat_enu;
		double [] cam_quats;
		for (int i = 0; i < reord.length; i++) {
			rot_reord = new Rotation(
					reord[i][0],
					reord[i][1],
					reord[i][2],
					reord[i][3],true);
//			quat_enu_rot = rot_reord.applyTo(quat_rot);
			quat_enu_rot = quat_rot.applyTo(rot_reord);
			quat_enu = new double[] {
					quat_enu_rot.getQ0(),
					quat_enu_rot.getQ1(),
					quat_enu_rot.getQ2(),
					quat_enu_rot.getQ3()};
			
			cam_quats =Imx5.quaternionImsToCam(
					quat_enu,
					ims_mount_atr,
					ims_ortho);
			double [] cam_xyzs = Imx5.applyQuternionTo(cam_quats, enu, false);
			System.out.println (i+": "+
			cam_xyzs[0]+", "+ cam_xyzs[1]+", "+ cam_xyzs[2]);
		}
		
    	return cam_xyz2;
    	
    	
    }
    
    
}
