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

import java.awt.Rectangle;
import java.io.File;
import java.io.FileOutputStream;
import java.io.IOException;
import java.io.OutputStream;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Comparator;
import java.util.Properties;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.stream.IntStream;

import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.common.PolynomialApproximation;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.ims.Did_ins_2;
import com.elphel.imagej.tileprocessor.ErsCorrection;
import com.elphel.imagej.tileprocessor.ImageDtt;
import com.elphel.imagej.tileprocessor.OpticalFlow;
import com.elphel.imagej.tileprocessor.QuadCLT;
import com.elphel.imagej.tileprocessor.TileProcessor;
import com.elphel.imagej.tileprocessor.TwoQuadCLT;
import com.elphel.imagej.vegetation.VegetationSegment;

import ij.IJ;
import ij.ImagePlus;
import ij.Prefs;

public class Cuas {
//	@Deprecated
	public static QuadCLT makeCenterClt( // not used
			QuadCLT []     quadCLTs,
			QuadCLT        ref_scene,
			int     []     range, // or null
			int            debugLevel
			) {
		QuadCLT center_CLT = ref_scene;
		if (!ref_scene.hasCenterClt()) {
			if (range == null) {
				range = new int [] {0, quadCLTs.length-1};
			}
			double [] cuas_xyz = new double[3];
			double [] cuas_atr = new double[3];
			double [][] cuas_xyzatr = {cuas_xyz, cuas_atr};
			double [][] center_ATR = CuasCenterLma.getCenterATR( // relative to ref_scene
					quadCLTs,    // QuadCLT [] quadCLTs,
					ref_scene,   // QuadCLT        ref_scene, //
					range,  //int     []     range,
    				true, // 			boolean    disable_AT_omegas,
					debugLevel); // int debugLevel);
			//maybe use later
			cuas_atr = new double [] { center_ATR[0][0], center_ATR[0][1], center_ATR[0][2]};
			double [] cuas_omegas = center_ATR[2];
			double cuas_index_avg = center_ATR[3][0];
			String center_name = QuadCLT.getCenterDirName(quadCLTs[quadCLTs.length - 1].getImageName());
			center_CLT = changeReference( // VERIFY HERE ***********************************
					quadCLTs,        // QuadCLT [] quadCLTs,
					ref_scene,       // QuadCLT    ref_old,
					null,            // QuadCLT    ref_new,
					center_name,     // String     name_new, 
					cuas_xyzatr,     // double[][] new_xyzatr);
					cuas_omegas,     // double []  omegas,
					cuas_index_avg); // double     index_avg) 
		}
		return center_CLT;
	}
	
	/**
	 * Get disparity for the center (virtual view) from the DSI data of the physical reference (one of) scene
	 * @param clt_parameters parameters
	 * @param ref_CLT     
	 * @param ref_combo_dsi combo_dsi for the reference (not the center/virtual) scene 
	 * @param condition_dsi
	 * @param pref_pXpYD
	 * @param debugLevel
	 * @return
	 */
	public static double [] getDisparityCenter(
			CLTParameters  clt_parameters,
			QuadCLT        ref_CLT, // where combo_dsi is should have hasCenterClt(),  (run makeCenterClt() before)
			double []      center_atr,
			double  [][]   ref_combo_dsi, // DSI data for the reference scene (or null to read it from file)
			boolean        condition_dsi,
    		double [][][]  pref_pXpYD, // pointer to ref_pXpYD, should be [1][][]
			int            debugLevel) {
		boolean batch_run =  clt_parameters.batch_run;
		boolean cuas_debug = clt_parameters.imp.cuas_debug;  // save debug images (and show them if not in batch mode)
		// null is handled by caller
		double [][] dls = new double[][] { 
			ref_combo_dsi[OpticalFlow.COMBO_DSN_INDX_DISP],    // 0 null
			ref_combo_dsi[OpticalFlow.COMBO_DSN_INDX_LMA],     // 2
			ref_combo_dsi[OpticalFlow.COMBO_DSN_INDX_STRENGTH] // 1
		};
		
		double [][] ds = new double [][] {dls[0],dls[2]}; // {disparity, strength}
		if (condition_dsi) {
			ds = OpticalFlow.conditionInitialDS( // only
					true,           // boolean        use_conf,       // use configuration parameters, false - use following  
					clt_parameters, // CLTParameters  clt_parameters,
					dls,            // double [][]    dls
					ref_CLT,      // QuadCLT        scene,
					debugLevel);			
		}
		// getRefPxPyD: pX, pY reference image X,Y corresponding to the uniform grid of the center view
		final double [][] ref_pXpYD = getRefPxPyD( // Use to interpolate disparity layers
				clt_parameters,    // CLTParameters  clt_parameters,
				true,              // boolean        mode_cuas,            		
				null,              // Rectangle      fov_tiles,
				OpticalFlow.ZERO3, // double []      stereo_xyz, // offset reference camera {x,y,z}
				center_atr, // OpticalFlow.ZERO3, // cuas_atr,          // double []      stereo_atr_in, // offset reference orientation (cuas)
				ds[0],             // double []      ref_disparity,			
				ref_CLT,        // QuadCLT        refCLT, // should be the same instance if one of quadCLTs
				debugLevel);       // int            debugLevel)
		//Filter ref_pXpYD from folds in pX, pY and other
		int     discard_border = clt_parameters.imp.cuas_discard_border;
		double  max_fold =       clt_parameters.imp.cuas_max_fold;
		int     min_in_row_col = clt_parameters.imp.cuas_min_in_row_col;
		if (debugLevel > -3) {
			System.out.println("createCenterClt(): filtering ref_pXpYD: discard_border="+discard_border+
					" pix, max_fold="+max_fold+" pix, min_in_row_col="+min_in_row_col+" tiles");
		}
		int width =  ref_CLT.getTilesX()*ref_CLT.getTileSize();
		int height = ref_CLT.getTilesY()*ref_CLT.getTileSize();

		boolean apply_window_filter = (discard_border > 0) || (max_fold > 0) || (min_in_row_col > 0);
		final Rectangle window = apply_window_filter ? (new Rectangle(discard_border,discard_border,width-2*discard_border,height-2*discard_border)): null; 
		if (window != null) {
			if (cuas_debug) {
				String debug_suffix="-disparity-pre-filtered";
				ImagePlus imp = ref_CLT.show_pXpYD(
						ref_pXpYD, // double [][] pXpYD,
						debug_suffix, // String      suffix,
						!batch_run);// boolean     show)
				if (imp != null) {
					String suffix ="-DISPARITY-PRE-FILTERED";
					ref_CLT.saveImagePlusInModelDirectory(
							suffix,          // String      suffix, // null - use title from the imp
							imp); // ImagePlus   imp)
				}
			}
			ref_CLT.windowPxPyD(	
					ref_pXpYD,           // final double [][] pXpYD,	
					window,          // final Rectangle window) // window in pixels!
					max_fold,        // final double max_fold)
					min_in_row_col); // final int         min_in_row_col,   // Minimal number of defined tiles in a row/column
		}


		double [] disparity_center = new double [ref_pXpYD.length];
		Arrays.fill(disparity_center, Double.NaN);
		for (int i = 0; i < ref_pXpYD.length; i++) if (ref_pXpYD[i] != null) {
			disparity_center[i] = ref_pXpYD[i][2];
		}
		// TODO: extra/interpolate, fill gaps in disparity_center
		int grow_width = Math.max(ref_CLT.getTilesX(), ref_CLT.getTilesY());
		int       num_passes = 20;
		double [][] debug_disparity_center = (cuas_debug)? (new double [2][]): null;
		if (debug_disparity_center != null) {
			debug_disparity_center[0] = disparity_center.clone();
		}
		disparity_center = TileProcessor.fillNaNs(
				disparity_center,  // final double [] data,
				null,            // final boolean [] prohibit,
				ref_CLT.getTilesX(),       // int       width,
				2 * grow_width,  // 100, // 2*width, // 16,           // final int grow,
				0.7,             // double    diagonal_weight, // relative to ortho
				num_passes,      // int       num_passes,
				0.03);           // final double     max_rchange, //  = 0.01 - does not need to be accurate
		
        if (debug_disparity_center != null) {
        	debug_disparity_center[1] = disparity_center.clone();
        }

        if (cuas_debug) {
        	String debug_suffix="-disparity-filtered";
        	ImagePlus imp=ref_CLT.show_pXpYD(
        			ref_pXpYD, // double [][] pXpYD,
        			debug_suffix, // String      suffix,
        			!batch_run);// boolean     show)
        	if (imp != null) {
				String suffix ="-DISPARITY-FILTERED";
				ref_CLT.saveImagePlusInModelDirectory(
						suffix,          // String      suffix, // null - use title from the imp
						imp); // ImagePlus   imp)
        	}        	
        	ImagePlus imp2=ShowDoubleFloatArrays.makeArrays(
        			debug_disparity_center,
        			ref_CLT.getTilesX(),
        			ref_CLT.getTilesY(),
        			ref_CLT.getImageName()+"-disparity-filtering",
        			new String[] {"before","after"});
        	if (imp != null) {
            	if (!batch_run) {
            		imp2.show();
            	}
				String suffix ="-DISPARITY-FILTERING";
				ref_CLT.saveImagePlusInModelDirectory(
						suffix,          // String      suffix, // null - use title from the imp
						imp2); // ImagePlus   imp)
        	}        	
        }
        if (pref_pXpYD != null) {
        	pref_pXpYD[0] = ref_pXpYD;
        }
		return disparity_center;
	}
	/**
	 * 
	 * @param clt_parameters
	 * @param quadCLTs
	 * @param ref_CLT center or ref, ref if !ref_CLT.hasCenterClt()
	 * @param range
	 * @param ref_combo_dsi
	 * @param condition_dsi
	 * @param sensor_mask
	 * @param new_average
	 * @param debugLevel
	 * @return
	 */
    public static QuadCLT createCenterClt( // assuming cuas_rotation is true
    		CLTParameters  clt_parameters,
    		QuadCLT []     quadCLTs,
    		QuadCLT        ref_CLT, // where combo_dsi is. Should have hasCenterClt()   (run makeCenterClt() before)
    		int     []     range, // or null
    		double  [][]   ref_combo_dsi, // DSI data for the reference scene (or null to read it from file)
    		boolean        condition_dsi,
    		int            sensor_mask, // -1 - all;
    		boolean        new_average,
    		int            debugLevel) {
//TODO: verify it still works with new reference_scene    	
		boolean batch_run = clt_parameters.batch_run;
    	boolean save_weights = true; // always
    	boolean clt_create = true; // create new variants
    	double  cuas_clt_variant =   clt_parameters.imp.cuas_clt_variant;   // 10;
    	double  cuas_clt_threshold = clt_parameters.imp.cuas_clt_threshold; // 20;
    	double  cuas_decay_average = clt_parameters.imp.cuas_decay_average; // 100;
    	double  cuas_keep_fraction = clt_parameters.imp.cuas_keep_fraction; // 0.9;
    	double  cuas_clt_decrease =  clt_parameters.imp.cuas_clt_decrease;  // 0.01;
		boolean cuas_debug =         clt_parameters.imp.cuas_debug;  // save debug images (and show them if not in batch mode)
		boolean inertial_only =      clt_parameters.imp.inertial_only;   // use internally
		boolean use_ims_rotation =   clt_parameters.imp.use_quat_corr;   // use internally (probably deprecated
		double [] quat_corr = (use_ims_rotation && (ref_CLT != null))? ref_CLT.getQuatCorr() : null;
    	if (!new_average && !ref_CLT.hasCenterClt()) {
    		System.out.println(" center_CLT does not have .center_clt data, bailing out");
    		return null; 
    	}
		if (ref_combo_dsi == null) {
			ref_combo_dsi =ref_CLT.restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky
			if (debugLevel > -3) {
				if (ref_combo_dsi != null) {
					System.out.println("Using *-INTER-INTRA-LMA.tiff.");
				}
			}
		}
		if (ref_combo_dsi == null) {
			ref_combo_dsi = ref_CLT.comboFromMain(ref_CLT.readDsiMain());
			if (debugLevel > -3) {
				if (ref_combo_dsi != null) {
					System.out.println("*-INTER-INTRA-LMA.tiff not available, got  *-DSI_MAIN instead");
				} else {
					System.out.println("ERROR: neighter *-INTER-INTRA-LMA.tiff, nor  *-DSI_MAIN are available");
					return null;
				}
			}
		}
				
    	if (range == null) {
    		range = new int [] {0, quadCLTs.length-1};
    	}
    	
    	QuadCLT center_CLT = ref_CLT.hasCenterClt() ? ref_CLT : null;
    	double [] center_atr= OpticalFlow.ZERO3;
    	if (center_CLT == null) {
    		// use image-aligned coordinates, not the IMU!
			if (debugLevel > -3) {
				System.out.println("createCenterClt(): Creating center virtual center image from image-adjsuted poses");
			}    		
    		double [][] center_ATR =  CuasCenterLma.getCenterATR(
    				quadCLTs, // QuadCLT [] quadCLTs,
    				ref_CLT, // QuadCLT    ref_scene,
    				range,          // int []     range,
    				true, // 			boolean    disable_AT_omegas,
    				debugLevel); // int        debugLevel)
    		
    		center_atr = center_ATR[0];
    		double [] center_omegas = null; // new double [] {-center_ATR[2][0],-center_ATR[2][1],-center_ATR[2][2]};
    		double cuas_index_avg = center_ATR[3][0]; 
//    		double [][] cuas_xyzatr = {OpticalFlow.ZERO3,{-center_atr[0],-center_atr[1],-center_atr[2]}};
    		double [][] cuas_xyzatr = {OpticalFlow.ZERO3, center_atr};
			String center_name = QuadCLT.getCenterDirName(quadCLTs[quadCLTs.length - 1].getImageName());
			center_CLT = changeReference( // VERIFY HERE ***********************************
					quadCLTs,        // QuadCLT [] quadCLTs,
					ref_CLT,         // QuadCLT    ref_old,
					null,            // QuadCLT    ref_new,
					center_name,     // String     name_new, 
					cuas_xyzatr,     // double[][] new_xyzatr);
					center_omegas,   // double []  omegas,
					cuas_index_avg); // double     index_avg) 
    	} else {
			if (debugLevel > -3) {
				System.out.println("createCenterClt(): Reusing available virtual center image.");
			}    		
    	}
    	
		double [][][]  pref_pXpYD = new double [1][][]; // pointer to ref_pXpYD, should be [1][][]
		//center_atr
    	double [] disparity_center = getDisparityCenter(
    			clt_parameters, // CLTParameters  clt_parameters,
    			ref_CLT,        // QuadCLT        ref_CLT, // where combo_dsi is should have hasCenterClt(),  (run makeCenterClt() before)
    			center_atr,     // double []      center_atr,
    			ref_combo_dsi,  // double  [][]   ref_combo_dsi, // DSI data for the reference scene (or null to read it from file)
    			condition_dsi,  // boolean        condition_dsi,
    			pref_pXpYD,     // double [][][]  pref_pXpYD, // pointer to ref_pXpYD, should be [1][][]
    			debugLevel);    // int            debugLevel)
    	
    	double [][] ref_pXpYD = pref_pXpYD[0];
    	
      	String cumulative_parent_dir_path = ref_CLT.getParentDir(); // getImagePath();
    	int num_colors = ref_CLT.getNumColors();
    	int tilesX =     ref_CLT.getTilesX();
    	int tilesY =     ref_CLT.getTilesY();
    	double dts =     ref_CLT.getTimeStamp();
    	CuasData cuasData = null;
    	// here we need cumulative parent data
    	if (!new_average) {
    		cuasData = CuasData.getCuasData( // may result in null !
    				true,            // boolean try_cumul,  // and save as such if only old style existed
    				cumulative_parent_dir_path, // String  full_path,  // ends with /vxx
    				num_colors,      // int     num_colors, // only used if restored from tiff, ignored for .cuas
    				tilesX,          // int     width,      // only used if restored from tiff, ignored for .cuas
    				dts,             // double  dts,        // time stamp as double, only used if restored from tiff, ignored for .cuas
    				debugLevel);     // int     debugLevel)
    		if (debugLevel > -4) {
    			if (cuasData != null) {
    				System.out.println("createCenterClt(): read cumulative parent data from "+cumulative_parent_dir_path);
    			} else {
    				System.out.println("createCenterClt(): FAILED to read cumulative parent data from "+cumulative_parent_dir_path);
    			}
    		}
    	} else {
    		if (debugLevel > -4) {
    				System.out.println("createCenterClt(): using new average");
    		}
    	}

    	CuasData newCuasData = getTDComboSceneSequence(
    			clt_parameters,      // CLTParameters  clt_parameters,
    			null, // ref_pXpYD,           // double [][]    ref_pXpYD,    		
    			save_weights,        // boolean        save_weights, // output corresponding weights for each data
    			sensor_mask,         // int            sensor_mask,
    			null,                // Rectangle      fov_tiles,
    			OpticalFlow.ZERO3,   // double []      stereo_xyz, // offset reference camera {x,y,z}
    			center_atr, // OpticalFlow.ZERO3,   // cuas_atr,            // double []      stereo_atr_in, // offset reference orientation (cuas)
    			disparity_center,    // ds[0],      // double []      ref_disparity,	// may be null if ref_pXpYD != null	
    			quadCLTs,            // QuadCLT []     quadCLTs,
    			center_CLT, // ref_CLT,             // QuadCLT        refCLT
    			cuasData,            // CuasData       cuasData, null or parent cumulative
    			clt_create,          // final boolean  clt_create, // create new variants
    			cuas_clt_variant,    // cuas_clt_threshold,  // final double   clt_threshold,
    			cuas_decay_average,  // final double   clt_decay,
    			cuas_clt_decrease,   // final double   clt_decrease, 
    			debugLevel);         // int            debugLevel)
// save new (not cumulative!) data
		String center_cuas_path = center_CLT.getImagePath()+Prefs.getFileSeparator() + center_CLT.getImageName()+CuasData.getCuasSuffix();
		try {
			newCuasData.writeCuasData (center_cuas_path);
	        if (debugLevel > -3) {
	        	System.out.println("createCenterClt(): wrote data to "+center_cuas_path);
	        }
		} catch (IOException e) {
	        if (debugLevel > -3) {
	        	System.out.println("createCenterClt(): ERROR writing data to "+center_cuas_path);
	        }
			// TODO Auto-generated catch block
			e.printStackTrace();
		}

    	if (cuas_debug) { // show_clt && !clt_parameters.batch_run) {
    		// Use null for one or both last arguments to disable debug
    		float [] fclt = newCuasData.collapse( // itself or parent
    				cuas_clt_threshold, // final double tolerance, // NaN works as infinity
    				cuas_decay_average, // final double decay,
    				dts,                // final double ts_now)
    				null, // clt_parameters, // null,               // CLTParameters clt_parameters,
    				null)[0];           // QuadCLT       center_CLT)
    		ImagePlus imp_new_clt= center_CLT.showCenterClt(
    				new float [][] {fclt},     // float [][] fclt, // may be null (do not save to center_CLT instance, just show/save)
    				clt_parameters,            // CLTParameters clt_parameters,
    				!batch_run); // true);          // 						boolean       show);
    		if (imp_new_clt != null) {
    			String suffix ="-CLT-NEW-SINGLE";
    			center_CLT.saveImagePlusInModelDirectory(
    					suffix,          // String      suffix, // null - use title from the imp
    					imp_new_clt); // ImagePlus   imp)
    		}
    	}

		// combine and create new cumulative:
		CuasData newCumulData = newCuasData;  // use existing
    	if (cuasData != null) {               // combine, each list for newCumulData tiles is longer or equal to cuasData ones
    		double []   dbg_weights = cuas_debug ? new double [tilesX * tilesY] : null;
    		newCumulData = combineAveragesCoherent(
    				 cuasData,           // CuasData          parentCuasData,
    				 newCuasData,        // CuasData          thisCuasData,   // each tile list starts with those from the  parent_cuas
    				 cuas_decay_average, // double            cuas_decay,    // seconds e times
    				 dbg_weights,               // final double []   dbg_weights, // should be null or double [tilesX*tilesY] - will return debug weight ratio (new to cumul)
    				 debugLevel);        // int debugLevel)
			if (cuas_debug) { // show_clt && !clt_parameters.batch_run) {
				ImagePlus imp_weights = ShowDoubleFloatArrays.makeArrays(
						dbg_weights,
						tilesX,
						tilesY,
						center_CLT.getImageName()+"-clt-weights-coherent");
				if (imp_weights != null) {
					String suffix ="-CLT-WEIGHTHS-COHERENT";
					center_CLT.saveImagePlusInModelDirectory(
							suffix,          // String      suffix, // null - use title from the imp
							imp_weights); // ImagePlus   imp)
					if (!batch_run) {
						imp_weights.show();
					}
				}
			}    	
    	}
     	if (cuas_keep_fraction < 1.0) { // only filter cumulative data
     		newCumulData =  filter(
     				newCumulData,        // final CuasData   cuasData,
    				cuas_decay_average, // final double     cuas_decay,    // seconds e times
    				cuas_keep_fraction, // final double     keep_fraction,
    				debugLevel);        //final int        debugLevel) {
    	}

		// save new CUMULATIVE data (will be used for next scene sequences)
		float [] fcumul_clt = newCumulData.collapse( // itself or parent
				cuas_clt_threshold, // final double tolerance, // NaN works as infinity
				cuas_decay_average, // final double decay,
				dts)[0];            // final double ts_now)
		center_CLT.setCenterClt(fcumul_clt); //	float [] clt)
		double [][] image_center = center_CLT.convertCenterClt(null);
//		System.out.print("createCenterClt(): ");
		center_CLT.setImageCenter(image_center);
		// 
		String center_cuas_cumul_path = center_CLT.getImagePath()+Prefs.getFileSeparator() + center_CLT.getImageName()+CuasData.getCuasCumulativeSuffix();
		try {
			newCumulData.writeCuasData (center_cuas_cumul_path);
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		

		final int transform_size = center_CLT.getTileSize();
		final double [][] center_combo_dsi = interpolateDSI(
				ref_pXpYD,      // final double [][] ref_pXpYD,
				ref_combo_dsi,  // combo_dsi_cli,  // final double [][] dsi_in,
				transform_size, // final int         transform_size,
				tilesX,         // final int         tilesX,
				tilesY);        //final int         tilesY);
		
		int [] slices_to_fill = {
				OpticalFlow.COMBO_DSN_INDX_DISP,
				OpticalFlow.COMBO_DSN_INDX_STRENGTH,
//				OpticalFlow.COMBO_DSN_INDX_LMA, // keep as is
//				OpticalFlow.COMBO_DSN_INDX_CHANGE, // may just relace nan with 0
				OpticalFlow.COMBO_DSN_INDX_DISP_FG, // Needed in setDSIFromCombo() !!!
///				OpticalFlow.COMBO_DSN_INDX_DISP_BG_ALL
				};
		fillNanDsi(
				ref_pXpYD,        // double [][] ref_pXpYD,
				center_combo_dsi, // double [][] dsi,
				center_CLT,       // QuadCLT     ref_clt,  // just for dimensions
				slices_to_fill,   // int []      slices)
				debugLevel);      // int         debugLevel)
		
		center_CLT.setDSIFromCombo(center_combo_dsi); // reformat
		String rslt_suffix = "-INTER-INTRA";
		if (!clt_parameters.correlate_lma){
			System.out.println("createCenterClt() BUG??? clt_parameters.correlate_lma="+clt_parameters.correlate_lma);
			clt_parameters.correlate_lma = true;
		}
		rslt_suffix += (clt_parameters.correlate_lma?"-LMA":"-NOLMA");
		center_CLT.saveDoubleArrayInModelDirectory( // error
				rslt_suffix,                  // String      suffix,
				OpticalFlow.COMBO_DSN_TITLES, // combo_dsn_titles_full, // null,          // String []   labels, // or null
				center_combo_dsi,             // dbg_data,         // double [][] data,
				tilesX,                       // int         width,
				tilesY);                      // int         height)
		
		// center_CLT.saveCenterClt();
		//***************************************************
		//FIXME: implement combine DSI!

		// Inject averaged LLA into center_CLT.did_ins_2 and persist to IMS file.
		// center_CLT is a virtual scene with no raw IMS logs; we average the camera
		// position from the real sequence scenes and save only the did_ins_2 entry so
		// that restoreCenterClt() can load it later without touching EventLogger.
		{
			Did_ins_2 ref_did_ins_2 = null;
			double lat_sum = 0, lon_sum = 0, alt_sum = 0;
			int lla_count = 0;
			for (int i = range[0]; i <= range[1]; i++) {
				if (quadCLTs[i] != null && quadCLTs[i].did_ins_2 != null) {
					double [] lla = quadCLTs[i].did_ins_2.lla;
					if (lla != null && (lla[0] != 0.0 || lla[1] != 0.0)) {
						if (ref_did_ins_2 == null) ref_did_ins_2 = quadCLTs[i].did_ins_2;
						lat_sum += lla[0];
						lon_sum += lla[1];
						alt_sum += lla[2];
						lla_count++;
					}
				}
			}
			if (ref_did_ins_2 != null) {
				// Clone ref_did_ins_2 (keeps quaternion/timing) and override LLA with sequence average
				Properties center_ims_props = new Properties();
				ref_did_ins_2.setProperties("did_ins_2-", center_ims_props);
				Did_ins_2 center_did_ins_2 = new Did_ins_2("did_ins_2-", center_ims_props);
				center_did_ins_2.lla[0] = lat_sum / lla_count;
				center_did_ins_2.lla[1] = lon_sum / lla_count;
				center_did_ins_2.lla[2] = alt_sum / lla_count;
				center_CLT.did_ins_2 = center_did_ins_2;
				// Write minimal IMS XML: ims_offs, gmt_plus, did_ins_2 only.
				// Do not call saveIms() — it tries to populate did_ins_1/did_pimu etc. via EventLogger.
				Properties ims_properties = new Properties();
				ims_properties.setProperty("ims_offs", clt_parameters.imp.ims_offset + "");
				ims_properties.setProperty("gmt_plus", clt_parameters.imp.gmt_plus + "");
				center_did_ins_2.setProperties("did_ins_2-", ims_properties);
				// Mirror saveIms() for physical scenes: store above the version subdir.
				// center_CLT.getImagePath() == <center_dir>/v04, so getParent() == <center_dir>
				String center_scene_dir = new File(center_CLT.getImagePath()).getParent();
				String ims_path = center_scene_dir + Prefs.getFileSeparator() +
						center_CLT.getImageName() + center_CLT.correctionsParameters.imsSuffix;
				try (OutputStream os = new FileOutputStream(ims_path)) {
					ims_properties.storeToXML(os, "last updated " + new java.util.Date(), "UTF8");
					center_CLT.ims_offs = clt_parameters.imp.ims_offset;
					center_CLT.gmt_plus = clt_parameters.imp.gmt_plus;
					center_CLT.ims_last_path = ims_path;
				} catch (IOException e) {
					System.out.println("createCenterClt(): ERROR saving center IMS file: " + ims_path);
					e.printStackTrace();
				}
				System.out.println("createCenterClt(): center_CLT did_ins_2 averaged from " + lla_count +
						" scenes: lat=" + IJ.d2s(center_did_ins_2.lla[0], 6) +
						" lon=" + IJ.d2s(center_did_ins_2.lla[1], 6) +
						" alt=" + IJ.d2s(center_did_ins_2.lla[2], 1) + ", saved to " + ims_path);
			} else {
				System.out.println("createCenterClt(): WARNING: no valid LLA in sequence scenes, center_CLT.did_ins_2 not set");
			}
		}

		return center_CLT;
    }
    
    
    /**
     * Only update template for correlation, do not save 
     * @param clt_parameters
     * @param quadCLTs
     * @param center_CLT
     * @param range
     * @param ref_combo_dsi
     * @param condition_dsi
     * @param sensor_mask
     * @param cuas_debug
     * @param debugLevel
     * @return
     */
    public static QuadCLT stepCenterClt( // assuming cuas_rotation is true
    		CLTParameters  clt_parameters,
    		QuadCLT []     quadCLTs,
    		QuadCLT        center_CLT, // where combo_dsi is
    		int     []     range, // or null
    		double  [][]   ref_combo_dsi, // DSI data for the reference scene (or null to read it from file)
    		boolean        condition_dsi,
    		int            sensor_mask, // -1 - all;
    		boolean        cuas_debug, //  =         clt_parameters.imp.cuas_debug;  // save debug images (and show them if not in batch mode)
    		int            debugLevel) {
    	if (!center_CLT.hasCenterClt()) {
    		System.out.println("stepCenterClt(): center_CLT does not have .center_clt data, bailing out");
    		return null;
    	}
		if (ref_combo_dsi == null) {
			ref_combo_dsi =center_CLT.restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky
			if (debugLevel > -3) {
				if (ref_combo_dsi != null) {
					System.out.println("stepCenterClt(): Using *-INTER-INTRA-LMA.tiff.");
				}
			}
		}
		if (ref_combo_dsi == null) {
			System.out.println("stepCenterClt(): ref_combo_dsi== null, bailing out (will retry when *-INTER-INTRA-LMA will be available");
			return null;
		}
    	
    	int num_colors = center_CLT.getNumColors();
    	int tile_size =  center_CLT.getTileSize();
    	int tilesX =     center_CLT.getTilesX();
    	double dts =     center_CLT.getTimeStamp();
    	CuasData cuasData = new CuasData (
				num_colors,                // final int      num_colors,
				tilesX * tile_size,        // final int      width, // should be multiple of width
				center_CLT.getCenterClt(), // final float [] fclt,
				new float [] {1.0f},       // fclt_weights, // final float [] fclt_weights, // weights,weights[1] - common weight, null - common weight== 1,
				dts);
		boolean batch_run = clt_parameters.batch_run;
    	// boolean save_weights = true; // always
    	double  cuas_clt_variant =   clt_parameters.imp.cuas_clt_variant;   // 10;
    	double  cuas_clt_threshold = clt_parameters.imp.cuas_clt_threshold; // 20;
    	double  cuas_decay_average = clt_parameters.imp.cuas_decay_average; // 100;
    	double  cuas_clt_decrease =  clt_parameters.imp.cuas_clt_decrease;  // 0.01;
    	if (range == null) {
    		range = new int [] {0, quadCLTs.length-1};
    	}
    	
    	
		double [][][]  pref_pXpYD = new double [1][][]; // pointer to ref_pXpYD, should be [1][][]

    	double [] disparity_center = getDisparityCenter(
    			clt_parameters, // CLTParameters  clt_parameters,
    			center_CLT,     // QuadCLT        center_CLT, // where combo_dsi is should have hasCenterClt(),  (run makeCenterClt() before)
				new double[3], // double []      center_atr,
    			ref_combo_dsi,  // double  [][]   ref_combo_dsi, // DSI data for the reference scene (or null to read it from file)
    			condition_dsi,  // boolean        condition_dsi,
    			pref_pXpYD,     // double [][][]  pref_pXpYD, // pointer to ref_pXpYD, should be [1][][]
    			debugLevel);    // int            debugLevel)

    	CuasData newCuasData = getTDComboSceneSequence(
    			clt_parameters,      // CLTParameters  clt_parameters,
    			null,                // ref_pXpYD,           // double [][]    ref_pXpYD,    		
    			false, // save_weights,        // boolean        save_weights, // output corresponding weights for each data
    			sensor_mask,         // int            sensor_mask,
    			null,                // Rectangle      fov_tiles,
    			OpticalFlow.ZERO3,   // double []      stereo_xyz, // offset reference camera {x,y,z}
    			OpticalFlow.ZERO3,   // cuas_atr,            // double []      stereo_atr_in, // offset reference orientation (cuas)
    			disparity_center,    // ds[0],      // double []      ref_disparity,	// may be null if ref_pXpYD != null	
    			quadCLTs,            // QuadCLT []     quadCLTs,
    			center_CLT,          // ref_scene,           // QuadCLT        refCLT, // should be the same instance if one of quadCLTs
    			cuasData,            // CuasData       cuasData, null or parent cumulative
    			false, // clt_create,// final boolean  clt_create, // create new variants
    			cuas_clt_variant,    // cuas_clt_threshold,  // final double   clt_threshold,
    			cuas_decay_average,  // final double   clt_decay,
    			cuas_clt_decrease,   // final double   clt_decrease, 
    			debugLevel);         // int            debugLevel)
    	fillNewGaps( // copy from parent
    			cuasData,     // final CuasData    parentCuasData,
    			newCuasData,  // final CuasData    thisCuasData,   // each tile list starts with those from the  parent_cuas
    			debugLevel);  // final int         debugLevel)

		float [] fclt = newCuasData.collapse( // itself or parent
				cuas_clt_threshold, // final double tolerance, // NaN works as infinity
				cuas_decay_average, // final double decay,
				dts)[0];            // final double ts_now)
		center_CLT.setCenterClt(fclt); //	float [] clt)
		
    	if (cuas_debug) { // show_clt && !clt_parameters.batch_run) {
    		ImagePlus imp_new_clt= center_CLT.showCenterClt(
    				new float [][] {fclt},     // float [][] fclt, // may be null (do not save to center_CLT instance, just show/save)
    				clt_parameters,            // CLTParameters clt_parameters,
    				!batch_run); // true);          // 						boolean       show);
    		if (imp_new_clt != null) {
    			String suffix ="-CLT-STEP-NEW-SINGLE";
    			center_CLT.saveImagePlusInModelDirectory(
    					suffix,          // String      suffix, // null - use title from the imp
    					imp_new_clt); // ImagePlus   imp)
    		}
    	}
    	return center_CLT;
    }

    /**
     * Rebuild the center IMAGE from the current in-memory scene image data (e.g. after
     * subtracting a just-computed FPN) WITHOUT persisting or mutating anything: no
     * .cuas / -CLT / DSI / IMS files are written and center_CLT.center_clt /
     * center_CLT.image_center are left untouched. Always a NEW average - no parent
     * cumulative is folded in (unlike stepCenterClt() which seeds from the current
     * center and would keep its content residual). Used by the FPN second
     * back-propagation iteration in CorrectionFPN.cuasSubtractFpn(), where the rebuilt
     * center is only a temporary measurement reference.
     * By Claude on 07/06/2026
     * @param clt_parameters processing parameters
     * @param quadCLTs scenes to accumulate; pass a slice covering an integer number of
     *        camera rotations so azimuth sectors are equally weighted
     * @param center_CLT existing center (geometry/poses reference), must have center CLT
     * @param disparity_center center-grid disparity (from backPrepareCenter()/getDisparityCenter())
     * @param sensor_mask -1 - all
     * @param debugLevel debug level
     * @return image-domain center as convertCenterClt() returns, or null on failure
     */
    public static double [][] rebuildCenterImageInMemory( // By Claude on 07/06/2026
    		CLTParameters  clt_parameters,
    		QuadCLT []     quadCLTs,
    		QuadCLT        center_CLT,
    		double []      disparity_center,
    		int            sensor_mask, // -1 - all;
    		int            debugLevel) {
    	if (!center_CLT.hasCenterClt()) {
    		System.out.println("rebuildCenterImageInMemory(): center_CLT does not have .center_clt data, bailing out");
    		return null;
    	}
    	double  cuas_clt_variant =   clt_parameters.imp.cuas_clt_variant;   // 10;
    	double  cuas_clt_threshold = clt_parameters.imp.cuas_clt_threshold; // 20;
    	double  cuas_decay_average = clt_parameters.imp.cuas_decay_average; // 100;
    	double  cuas_keep_fraction = clt_parameters.imp.cuas_keep_fraction; // 0.9;
    	double  cuas_clt_decrease =  clt_parameters.imp.cuas_clt_decrease;  // 0.01;
    	double  dts =                center_CLT.getTimeStamp();
    	CuasData newCuasData = getTDComboSceneSequence(
    			clt_parameters,      // CLTParameters  clt_parameters,
    			null,                // double [][]    ref_pXpYD,
    			true,                // boolean        save_weights,
    			sensor_mask,         // int            sensor_mask,
    			null,                // Rectangle      fov_tiles,
    			OpticalFlow.ZERO3,   // double []      stereo_xyz, // offset reference camera {x,y,z}
    			OpticalFlow.ZERO3,   // double []      stereo_atr_in, // center already exists - zero ATR
    			disparity_center,    // double []      ref_disparity,
    			quadCLTs,            // QuadCLT []     quadCLTs,
    			center_CLT,          // QuadCLT        refCLT,
    			null,                // CuasData       cuasData - no parent cumulative: new average only
    			true,                // final boolean  clt_create, // create new variants
    			cuas_clt_variant,    // final double   clt_threshold,
    			cuas_decay_average,  // final double   clt_decay,
    			cuas_clt_decrease,   // final double   clt_decrease,
    			debugLevel);         // int            debugLevel)
    	if (newCuasData == null) {
    		System.out.println("rebuildCenterImageInMemory(): getTDComboSceneSequence() failed");
    		return null;
    	}
    	if (cuas_keep_fraction < 1.0) { // same filtering as createCenterClt() applies before collapse
    		newCuasData = filter(
    				newCuasData,        // final CuasData   cuasData,
    				cuas_decay_average, // final double     cuas_decay,    // seconds e times
    				cuas_keep_fraction, // final double     keep_fraction,
    				debugLevel);        // final int        debugLevel)
    	}
    	float [] fclt = newCuasData.collapse(
    			cuas_clt_threshold, // final double tolerance, // NaN works as infinity
    			cuas_decay_average, // final double decay,
    			dts)[0];            // final double ts_now)
    	return center_CLT.convertCenterClt(new float [][] {fclt}); // does NOT touch center_CLT state
    }




    public static void fillNanDsi(
			double [][] ref_pXpYD,
			double [][] dsi,
			QuadCLT     ref_clt, // just for dimensions
			int []      slices,
			int         debugLevel)  {
        int grow_width = Math.max(ref_clt.getTilesX(), ref_clt.getTilesY());
        int       num_passes = 20;
        for (int slice:slices) if ((slice < dsi.length) && (dsi[slice] != null)){
        	double [] data = dsi[slice]; // .clone(); // not needed to clone
        	for (int i = 0; i < ref_pXpYD.length; i++) {
        		if (ref_pXpYD[i] == null) {
        			data[i] = Double.NaN; // for strength layers that are 0-s
        		}
        	}
    		if (debugLevel > -3) {
    			System.out.println("fillNanDsi(); starting for slice "+slice+" ("+OpticalFlow.COMBO_DSN_TITLES[slice]+")");
    		}
        	dsi[slice] = TileProcessor.fillNaNs(
        			data,  // final double [] data,
        			null,            // final boolean [] prohibit,
        			ref_clt.getTilesX(),       // int       width,
        			2 * grow_width,  // 100, // 2*width, // 16,           // final int grow,
        			0.7,             // double    diagonal_weight, // relative to ortho
        			num_passes,      // int       num_passes,
        			0.03);           // final double     max_rchange, //  = 0.01 - does not need to be accurate
        	if (debugLevel > -3) {
        		System.out.println("fillNanDsi(); done for slice "+slice+" ("+OpticalFlow.COMBO_DSN_TITLES[slice]+")");
        	}
        }
        return;
    }
    
    public static double [][] interpolateDSI( // uses combo_dsi, not this.dsi
			final double [][] ref_pXpYD,
			final double [][] combo_dsi_in,
			final int         transform_size,
			final int         tilesX,
			final int         tilesY){
		final double [][] combo_dsi = new double [combo_dsi_in.length][];
		final int tiles = tilesY * tilesX;
		//COMBO_DSN_NONNAN
		boolean [] non_nan = new boolean[combo_dsi_in.length];
		for (int indx:OpticalFlow.COMBO_DSN_NONNAN) {
			if (indx < non_nan.length) {
				non_nan[indx] = true;
			}
		}
		for (int slice = 0; slice < combo_dsi_in.length; slice++) if (combo_dsi_in[slice] != null) {
			combo_dsi[slice] = new double [tiles];
			if (!non_nan[slice]) {
				Arrays.fill(combo_dsi[slice], Double.NaN);// improve
			}
		}
		final Thread[] threads = ImageDtt.newThreadArray(ImageDtt.THREADS_MAX);
		final AtomicInteger ai = new AtomicInteger(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
//					PolynomialApproximation pa = new PolynomialApproximation();		
					for (int nTile = ai.getAndIncrement(); nTile < tiles; nTile = ai.getAndIncrement()) if (ref_pXpYD[nTile] != null){
						double [] pXpYD = ref_pXpYD[nTile];
						double px = pXpYD[0];
						double py = pXpYD[1];
						double tx = px/transform_size-0.5;
						double ty = py/transform_size-0.5;
						int tx0 = (int) Math.floor(tx);
						int ty0 = (int) Math.floor(ty);
						int tx1 = tx0+1;
						int ty1 = ty0+1;
						double kx = tx-tx0;
						double ky = ty-ty0;
						if (tx0 < 0) {
							tx0 = 0;
							tx1 = tx0+1;
							kx = 0;
						} else if (tx1 >= tilesX) {
							tx1 = tilesX - 1;
							tx0 = tx1-1;
							kx = 1.0;
						}
						if (ty0 < 0) {
							ty0 = 0;
							ty1 = ty0+1;
							ky = 0;
						} else if (ty1 >= tilesY) {
							ty1 = tilesY - 1;
							ty0 = ty1-1;
							ky = 1.0;
						}
						for (int slice = 0; slice < combo_dsi_in.length; slice++) 	if (combo_dsi_in[slice] != null) {
							double [][] d4 = {
									{combo_dsi_in[slice][tilesX*ty0 + tx0],combo_dsi_in[slice][tilesX*ty0 + tx1]},
									{combo_dsi_in[slice][tilesX*ty1 + tx0],combo_dsi_in[slice][tilesX*ty1 + tx1]},
							};
							int dn = 0;
							for (int i = 0; i < 2; i++) { 
								for (int j = 0; j < 2; j++) {
									if (Double.isNaN(d4[i][j])) {
										dn |= 1 << (2*i + j);
									}
								}
							}
							if (dn == 15) continue; // all NaNs
							if (dn != 0) { // fill NaNs
								for (int i = 0; i < 2; i++) { 
									for (int j = 0; j < 2; j++) {
										if ((dn & (1 << (2*i + j))) != 0) { // is NaN
											int hnb =  (1 << (2*i + 1 -j)); // horizontal neighbor bit
											int vnb =  (1 << (2 - 2*i +j)); // horizontal neighbor
											if ((dn & hnb) == 0) { // non-NaN
												if ((dn & vnb) == 0) { // non-NaN
													// use average of 2 neighbors, ignore diagonal
													d4[i][j] = 0.5*(d4[i][1-j]+d4[1-i][j]);
												} else { // if ((dn & vnb) == 0) {
													d4[i][j] = d4[i][1-j]; // use horizontal neighbor
												}
											} else { // if ((dn & hnb) == 0)
												if ((dn & vnb) == 0) {  // non-NaN
													d4[i][j] = d4[1-i][j]; // use vertical neighbor
												} else { // if ((dn & vnb) == 0) {
													int dnb =  (1 << (2 - 2*i + 1 - j)); // diagonal neighbor
													d4[i][j] = d4[1-i][1-j]; // copy diagonal
												}
											}
										}
									}
								}
							}
							combo_dsi[slice][nTile] =
									(1-kx)*(1-ky)*d4[0][0] +
									(  kx)*(1-ky)*d4[0][1] +
									(1-kx)*(  ky)*d4[1][0] +
									(  kx)*(  ky)*d4[1][1];
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return combo_dsi;
	}

    /**
     * If cuasData != null, then each tile of the new CuasData instance will have first variants
     * corresponding to cuasData (some may be empty). New variants may be added to the end of new lists 
     * @param clt_parameters
     * @param ref_pXpYD
     * @param save_weights
     * @param sensor_mask
     * @param fov_tiles
     * @param stereo_xyz
     * @param stereo_atr_in
     * @param ref_disparity
     * @param quadCLTs
     * @param refCLT
     * @param cuasData
     * @param clt_create
     * @param clt_threshold
     * @param clt_decay
     * @param clt_decrease
     * @param debugLevel
     * @return
     */
    public static CuasData getTDComboSceneSequence(
    		CLTParameters  clt_parameters,
    		double [][]    ref_pXpYD,    // TODO: Use disparity, ignore pXpYD?		
    		final boolean  save_weights, // output corresponding weights for each data
    		final int      sensor_mask,
    		Rectangle      fov_tiles,
    		double []      stereo_xyz, // offset reference camera {x,y,z}
    		double []      stereo_atr_in, // offset reference orientation (cuas)
    		double []      ref_disparity, // null			
    		QuadCLT []     quadCLTs,
    		QuadCLT        refCLT, // center_CLT
    		CuasData       cuasData,
    		final boolean  clt_create, // create new variants
    		final double   clt_threshold,
    		final double   clt_decay,
    		final double   clt_decrease,
    		int            debugLevel) {

    	boolean batch_run =clt_parameters.batch_run; // may be modified for debug
		boolean cuas_debug =  clt_parameters.imp.cuas_debug;  // save debug images (and show them if not in batch mode)
    	boolean debug_pxpyd = cuas_debug; //   && !batch_run;
    	final double clt_threshold2 = clt_threshold*clt_threshold;
    	int dbg_slices = 3;
    	final int num_colors = refCLT.getNumColors();
    	final int tilesX =     refCLT.getTilesX();
    	final int tilesY =     refCLT.getTilesY();
//    	final double[][] tile_diffs = (debug_pxpyd && (cuasData != null)) ? new double [quadCLTs.length][tilesX*tilesY]:null;
    	final double[][] tile_diffs = (debug_pxpyd ) ? new double [quadCLTs.length][tilesX*tilesY]:null;
    	if (tile_diffs != null) {
    		for (int i = 0; i < tile_diffs.length; i++) {
    			Arrays.fill(tile_diffs[i], Double.NaN);
    		}
    	}
//    	final double [][] dnum_vars = (debug_pxpyd && (cuasData != null)) ? new double [quadCLTs.length][tilesX*tilesY]:null;
    	final double [][] dnum_vars = (debug_pxpyd ) ? new double [quadCLTs.length][tilesX*tilesY]:null;
    	
    	
    	double [][][] dbg_PxPyD = debug_pxpyd? (new double [dbg_slices][quadCLTs.length][]):null;
    	double [][][] dbg_PxPyD_slice = debug_pxpyd? (new double [1][][]):null;
    	final float [][] dbg_fclt = debug_pxpyd ? new float [quadCLTs.length][] : null;
        int dbg_scene = -95;
        if (ref_pXpYD == null) {
        	ref_pXpYD = OpticalFlow.transformToScenePxPyD( // now should work with offset ref_scene
        			fov_tiles,             // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
        			ref_disparity,         // final double []   disparity_ref, // invalid tiles - NaN in disparity
        			OpticalFlow.ZERO3,     // stereo_xyz, // ZERO3,                // final double []   scene_xyz, // camera center in world coordinates
        			OpticalFlow.ZERO3,     // stereo_atr, // ZERO3,                // final double []   scene_atr, // camera orientation relative to world frame
        			refCLT,                // final QuadCLT     scene_QuadClt,
        			refCLT,                // final QuadCLT     reference_QuadClt, // now - may be null - for testing if scene is rotated ref
        			ImageDtt.THREADS_MAX);          // int               threadsMax)
        }	
        if (debug_pxpyd) {
        	ImagePlus imp = refCLT.show_pXpYD(
        			ref_pXpYD, // double [][] pXpYD,
        			"-getTDComboSceneSequence", // String      suffix,
        			!batch_run); // boolean show)
        	if (imp != null) {
        		//refCLT
        		String suffix ="-getTDComboSceneSequence";
        		refCLT.saveImagePlusInModelDirectory(
        				suffix,          // String      suffix, // null - use title from the imp
        				imp); // ImagePlus   imp)

        	}
        }
        
    	double [] stereo_atr = (stereo_atr_in != null)? stereo_atr_in: OpticalFlow.ZERO3; // maybe later play with rotated camera
    	boolean mode_cuas =    (stereo_atr[0] != 0) || (stereo_atr[1] != 0) || (stereo_atr[2] != 0);
        double [][] ref_pXpYD_or_null = null; // mode_cuas ? ref_pXpYD : null; // debugging cuas mode keeping old
		boolean mb_en =               clt_parameters.imp.mb_en && (fov_tiles==null);
		double  mb_tau =              clt_parameters.imp.mb_tau;      // 0.008; // time constant, sec
		double  mb_max_gain =         clt_parameters.imp.mb_max_gain; // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
		int     cuas_discard_border = clt_parameters.imp.cuas_discard_border;
		double cuas_max_fold =        clt_parameters.imp.cuas_max_fold;
		int cuas_min_in_row_col =     clt_parameters.imp.cuas_min_in_row_col;
		ErsCorrection ers_reference = refCLT.getErsCorrection();
        int sc0 = -1;
		for (int nscene =  0; nscene < quadCLTs.length ; nscene++) if (quadCLTs[nscene] != null){
			sc0 = nscene;
			break;
		}
		
//		final int num_slices = merge_all? 1 : quadCLTs[sc0].getNumSensors();
		final int num_tiles = quadCLTs[sc0].getNumTiles(false);
		final int tile_length = quadCLTs[sc0].getCltTileLength(); // includes color
		final CuasData newCuasData = (cuasData != null)? cuasData.cloneEmpty():
			new CuasData (
					num_colors, // int num_colors,
					tilesX,     // int width, // should be multiple of width
					tilesY);  // int height) {
        boolean show_src = false; // cuas_debug;
		final Thread[] threads = ImageDtt.newThreadArray(ImageDtt.THREADS_MAX);
		final AtomicInteger ai = new AtomicInteger(0);
		for (int nscene =  0; nscene < quadCLTs.length ; nscene++) if (quadCLTs[nscene] != null){
			final int fnscene = nscene; 
			if (nscene== dbg_scene) {
				System.out.println("renderSceneSequence(): nscene = "+nscene);
			}
			if (show_src) {
		    	int tile_size =  refCLT.getTileSize();
				String title = quadCLTs[nscene].getImageName()+"-SRC";
				double [][] dbg_data = new double [quadCLTs[nscene].getImageData().length][];
				String [] titles = new String [dbg_data.length];
				for (int i = 0; i < dbg_data.length; i++) {
					titles[i] = "SENSOR-"+i;
					dbg_data[i] = quadCLTs[nscene].getImageData()[i][0];
				}
				ShowDoubleFloatArrays.showArrays(
						dbg_data,
						  tilesX * tile_size,
						  tilesY * tile_size,
						  true,
						  title,
						  titles);
			}
			String ts = quadCLTs[nscene].getImageName();
			final double dts = quadCLTs[nscene].getTimeStamp();
			double []   scene_xyz = OpticalFlow.ZERO3;
			double []   scene_atr = OpticalFlow.ZERO3;
			if (quadCLTs[nscene] != refCLT) { // Check even for raw, so video frames will match in all modes 
				scene_xyz = ers_reference.getSceneXYZ(ts);  // saved @ reference, relative to reference
				scene_atr = ers_reference.getSceneATR(ts);  // saved @ reference, relative to reference
				if ((scene_atr==null) || (scene_xyz == null)) {
					continue;
				}
				double []   scene_ers_xyz_dt = ers_reference.getSceneErsXYZ_dt(ts);
				double []   scene_ers_atr_dt = ers_reference.getSceneErsATR_dt(ts);
				quadCLTs[nscene].getErsCorrection().setErsDt(
						scene_ers_xyz_dt, // double []    ers_xyz_dt,
						scene_ers_atr_dt); // double []    ers_atr_dt)(ers_scene_original_xyz_dt);
			}
			if (!mode_cuas && (stereo_xyz != null)) { // offset all, including reference scene - now always, it is never null
				double [][] combo_xyzatr = ErsCorrection.combineXYZATR(
						stereo_xyz,  // double [] reference_xyz,
						stereo_atr,  // double [] reference_atr, 
						scene_xyz,   // double [] scene_xyz,
						scene_atr);  // double [] scene_atr) 
				scene_xyz = combo_xyzatr[0];
				scene_atr = combo_xyzatr[1];
			}
			int sm = sensor_mask;
			float [][] fclt = null;
			double [][] dxyzatr_dt = null;
			// should get velocities from HashMap at reference scene from timestamp , not re-calculate.
			if (mb_en) {
				dxyzatr_dt = new double[][] { // for all, including ref
					quadCLTs[nscene].getErsCorrection().getErsXYZ_dt(),
					quadCLTs[nscene].getErsCorrection().getErsATR_dt()};				
			}
			
			
			if (mb_en && (dxyzatr_dt != null)) {
				double [][] motion_blur = OpticalFlow.getMotionBlur(
						refCLT, // 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,             // double []      camera_xyz,
						scene_atr,             // double []      camera_atr,
						dxyzatr_dt[0],         // double []      camera_xyz_dt,
						dxyzatr_dt[1],         // double []      camera_atr_dt,
						0,                     // int            shrink_gaps,  // will gaps, but not more that grow by this
						debugLevel);           // int            debug_level)
				fclt = QuadCLT.getTDCombo(
						sm,                  // final int         sensor_mask,
						true, // merge_all,           // final boolean     merge_channels,
						cuas_discard_border, // final int         discard_border,
						cuas_max_fold,       // final double      max_fold,
						cuas_min_in_row_col, // final int         min_in_row_col,   // Minimal number of defined tiles in a row/column
						
						null,                // final Rectangle   full_woi_in,      // show larger than sensor WOI (or null)
						clt_parameters,      // CLTParameters     clt_parameters,
						ref_disparity,       // double []         disparity_ref,
						ref_pXpYD_or_null,   // double [][]       ref_pXpYD,    // alternative to disparity_ref when reference is not uniform
						// 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,  //
						scene_xyz,           // final double []   scene_xyz, // camera center in world coordinates
						scene_atr,           // final double []   scene_atr, // camera orientation relative to world frame
						quadCLTs[nscene],    // final QuadCLT     scene,
						refCLT, // quadCLTs[ref_index], // final QuadCLT     ref_scene, // now - may be null - for testing if scene is rotated ref
						true,                // final boolean     show_nan,
						dbg_PxPyD_slice,     // final double [][][] dbg_PxPyD_slice,
						QuadCLT.THREADS_MAX, // int               threadsMax,
						debugLevel);         // final int         debugLevel) 

			} else {
				fclt = QuadCLT.getTDCombo(
						sm,                  // final int         sensor_mask,
						true, // merge_all,           // final boolean     merge_channels,
						cuas_discard_border, // final int         discard_border,
						cuas_max_fold,       // final double      max_fold,
						cuas_min_in_row_col, // final int         min_in_row_col,   // Minimal number of defined tiles in a row/column
						null,                // final Rectangle   full_woi_in,      // show larger than sensor WOI (or null)
						clt_parameters,      // CLTParameters     clt_parameters,
						ref_disparity,       // double []         disparity_ref,
						ref_pXpYD_or_null,   // double [][]       ref_pXpYD,    // alternative to disparity_ref when reference is not uniform
						// motion blur compensation 
						0,                   // double            mb_tau,      // 0.008; // time constant, sec
						0,                   // double            mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
						null,                // double [][]       mb_vectors,  //
						scene_xyz,           // final double []   scene_xyz, // camera center in world coordinates
						scene_atr,           // final double []   scene_atr, // camera orientation relative to world frame
						quadCLTs[nscene],    // final QuadCLT     scene,
						refCLT, // quadCLTs[ref_index], // final QuadCLT     ref_scene, // now - may be null - for testing if scene is rotated ref
						true,                // final boolean     show_nan,
						dbg_PxPyD_slice,     // final double [][][] dbg_PxPyD_slice,
						QuadCLT.THREADS_MAX, // int               threadsMax,
						debugLevel);         // final int         debugLevel) 
			}
			if (dbg_fclt != null) {
				dbg_fclt[nscene] = fclt[0];
			}
			if (dbg_PxPyD_slice != null) {
				for (int slice = 0; slice < dbg_PxPyD.length; slice++) { // 3
					dbg_PxPyD[slice][nscene] = new double [dbg_PxPyD_slice[0].length]; // tiles
					Arrays.fill(dbg_PxPyD[slice][nscene], Double.NaN);
				}
				for (int ntile = 0; ntile < dbg_PxPyD_slice[0].length; ntile++) if (dbg_PxPyD_slice[0][ntile] != null) {
					for (int slice = 0; slice < dbg_PxPyD.length; slice++) {
						dbg_PxPyD[slice][nscene][ntile] = dbg_PxPyD_slice[0][ntile][slice];
					}
				}
			}
			final float [] ffclt = fclt[0];
			if (dbg_fclt != null) {
				dbg_fclt[nscene] = ffclt;
			}
			ai.set(0);
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					public void run() {
						for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) {
							int fclt_offs = nTile * tile_length;
							boolean no_nans = true;
							check_nan: {
									int indx = fclt_offs;
									for (int i = 0; i < tile_length; i++) {
										if (Float.isNaN(ffclt[indx++])) {
											no_nans = false;
											break check_nan;
										}
									}
							}
							if (!no_nans) {
								continue;
							}
							ArrayList<CuasTile> tile_list =     newCuasData.getTileData(nTile);
							ArrayList<CuasTile> tile_ref_list = (cuasData != null) ? cuasData.getTileData(nTile): null;
							if (tile_ref_list == null) {
								tile_ref_list = new ArrayList<CuasTile>(0); 
							}
							CuasTile tile = new CuasTile(
									num_colors, // int num_colors,
									dts, // double dts,
									1.0, // double weight,
									ffclt, // float [] clt_full,
									nTile); // int ntile)
							// fast not calculating difference
							if (clt_threshold <= 0) {
								if (tile_list.isEmpty()) {
									tile_list.add(tile);
									if (tile_diffs !=null) {
										tile_diffs[fnscene][nTile] = 0.0;
									}
								} else {
									if (tile_diffs !=null) {
										if (tile_list.get(0).getWeight() == 0) {
											tile_diffs[fnscene][nTile] = 0.0;
										} else {
											tile_diffs[fnscene][nTile] = Math.sqrt(tile_list.get(0).diffTile2(tile));
										}
									}
									tile_list.get(0).merge(tile, clt_decay); // replace or merge
								}
							} else { // need to calculate/compare differences
								double [] var_diffs2 = new double[tile_list.size()]; // tile_list.size() >= tile_ref_list.size()
								double best_diff2 = Double.POSITIVE_INFINITY;
								int best_index = 0;
								for (int i = 0; i < var_diffs2.length; i++) {
									CuasTile ref_tile = (i >= tile_ref_list.size()) ? tile_list.get(i) : tile_ref_list.get(i);
									var_diffs2[i] = ref_tile.diffTile2(tile);
									if (var_diffs2[i] < best_diff2) {
										best_diff2 = var_diffs2[i];
										best_index = i;
									}
								}
								if (best_diff2 <= clt_threshold2) { // OK to merge
									tile_list.get(best_index).merge(tile, clt_decay);
									if (tile_diffs !=null) {
										tile_diffs[fnscene][nTile] = Math.sqrt(best_diff2);
									}
								} else if (clt_create) {
									tile_list.add(tile);
									if (tile_diffs !=null) {
										tile_diffs[fnscene][nTile] = 0.0; // zero difference from the new value
									}
								}
							}
							
							if (dnum_vars != null) {
								dnum_vars[fnscene][nTile] = tile_list.size();
							}
							
						}
					}
				};
			}		      
			ImageDtt.startAndJoin(threads);
		}
		// calculate averages - seems it is now already done
		/*
		ai.set(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) {
						for (int slice = 0; slice < num_slices; slice++) { // normally just one
							int fclt_offs = nTile * tile_length;
							float [] sumFclt_slice = sumFclt[slice];
							int indx = fclt_offs;
							indx = fclt_offs;
							float w = sum_weights[slice][nTile]; // 1.0f; // constant weight
							if (w > 0) {
								for (int i = 0; i < tile_length; i++) {
									sumFclt_slice[indx++] /= w;
								}
							} else {
								for (int i = 0; i < tile_length; i++) {
									sumFclt_slice[indx++] =Float.NaN;
								}
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		*/
		if (dbg_fclt != null) {
			double [][] dbg_data = new double [quadCLTs.length][];
	    	int tile_size =  refCLT.getTileSize();
			String suffix ="-DBG_FCLT_SEQ";
			String [] titles = new String [quadCLTs.length];
			String title = refCLT.getImageName() + suffix;
			for (int nscene = 0; nscene < titles.length; nscene++) if (dbg_fclt[nscene] != null){
				titles[nscene] = quadCLTs[nscene].getImageName();
				dbg_data[nscene] = refCLT.convertCenterClt(
						new float[][] {dbg_fclt[nscene]})[0];//float [][] fclt) // null pointer
			}
			ImagePlus imp = ShowDoubleFloatArrays.makeArrays(
					dbg_data, // double[][] pixels,
					tilesX*tile_size,// int width,
					tilesY*tile_size,// int height,
					title,    // String title,
					titles);  // String [] titles)
			if (imp != null) {
				if (!batch_run) {
					imp.show();	
				}
				refCLT.saveImagePlusInModelDirectory(
						suffix, // String      suffix, // null - use title from the imp
						imp);   // ImagePlus   imp)

			}			
		}

		
		if (dbg_PxPyD != null) {
			String [] debug_frame_titles = {"pX","pY","Disparity"};
			String [] debug_titles = new String[quadCLTs.length];
			for (int nscene = 0; nscene < quadCLTs.length; nscene++) {
				debug_titles[nscene] = quadCLTs[nscene].getImageName();
			}
			String debugTitle = refCLT.getImageName()+"-pXpYD-discard"+cuas_discard_border+"_fold"+cuas_max_fold+"_min"+cuas_min_in_row_col;
			ImagePlus imp= ShowDoubleFloatArrays.showArraysHyperstack(
					dbg_PxPyD,          // double[][][] pixels, 
					refCLT.getTilesX(), // int          width, 
					debugTitle,         // String       title, "time_derivs-rt"+diff_time_rt+"-rxy"+diff_time_rxy,
					debug_titles,       // String []    titles, // all slices*frames titles or just slice titles or null
					debug_frame_titles, // String []    frame_titles, // frame titles or null
					!batch_run);        // boolean      show)
			if (imp != null) { // check it has ImageProcessor
				//refCLT
				String suffix ="-pXpYD-discard"+cuas_discard_border+"_fold"+cuas_max_fold+"_min"+cuas_min_in_row_col;
				refCLT.saveImagePlusInModelDirectory(
						suffix,          // String      suffix, // null - use title from the imp
						imp); // ImagePlus   imp)

			}
		}
		
		if (tile_diffs != null) {
			String [] diff_titles = new String [quadCLTs.length];
			for (int nscene = 0; nscene < diff_titles.length; nscene++) {
				diff_titles[nscene] = quadCLTs[nscene].getImageName();
			}
			String diff_title = refCLT.getImageName() + "-diffs";
			
			ImagePlus imp = ShowDoubleFloatArrays.makeArrays(
					tile_diffs, // double[][] pixels,
					tilesX, // int width,
					tilesY, // int height,
					diff_title, // String title,
					diff_titles); // String [] titles)
			if (imp != null) {
				if (!batch_run) {
					imp.show();	
				}
				String suffix ="-DIFFS";
				refCLT.saveImagePlusInModelDirectory(
						suffix, // String      suffix, // null - use title from the imp
						imp);   // ImagePlus   imp)
			}
		}
		if (dnum_vars != null) {
			String [] dnum_titles = new String [quadCLTs.length];
			for (int nscene = 0; nscene < dnum_titles.length; nscene++) {
				dnum_titles[nscene] = quadCLTs[nscene].getImageName();
			}
			String dnum_title = refCLT.getImageName() + "-dnum_vars";
			
			ImagePlus imp = ShowDoubleFloatArrays.makeArrays(
					dnum_vars, // double[][] pixels,
					tilesX, // int width,
					tilesY, // int height,
					dnum_title, // String title,
					dnum_titles); // String [] titles)
			if (imp != null) {
				if (!batch_run) {
					imp.show();	
				}
				String suffix ="-DNUM_VARS";
				refCLT.saveImagePlusInModelDirectory(
						suffix, // String      suffix, // null - use title from the imp
						imp);   // ImagePlus   imp)
			}
		}
		
    	return newCuasData; // new float [][][] {sumFclt,sum_weights};
    }
    
    
    
    /**
     * Convert scenes to a new reference (does not need to be one of the series, such as a virtual
     * center in CUAS mode)
     * @param quadCLTs array of scene instances, 
     * @param ref_old  old reference scene instance with a list of reference positions and velocities
     * @param ref_new  new reference scene (does not need to be one of quadCLTs)
     * @param name_new only needed if ref_new == null
     * @param new_xyzatr new reference offset and rotation relative to the old reference scene
     * @param omegas   drift rotations: azimuth, tilt, roll (radians per index 1) 
     * @param index_avg index for which drift rotations are zero
     * @return true if no errors
     */
    public static QuadCLT changeReference(
    		QuadCLT [] quadCLTs,
    		QuadCLT    ref_old,
    		QuadCLT    ref_new,
    		String     name_new, 
    		double[][] new_xyzatr,
    		double []  omegas,
    		double     index_avg) {
    	ErsCorrection ers_old_reference = ref_old.getErsCorrection();
    	double [][] inv_new_xyzatr = ErsCorrection.invertXYZATR(new_xyzatr);
    	if (ref_new == null) {
    		String ref_dir_path = ref_old.getX3dDirectory(name_new);
    		File cdir = new File(ref_dir_path);
    		ref_new =  new QuadCLT(ref_old, name_new); // empty scenes_poses
    		cdir.mkdirs();
    		ref_new.setImagePath(cdir.getPath());
    	}
    	ErsCorrection ers_new_reference = ref_new.getErsCorrection();
    	double [] inv_new_atr = inv_new_xyzatr[1].clone();
		for (int scene_index = 0; scene_index < quadCLTs.length; scene_index++) {
			if (omegas != null) {
				double time_drift = scene_index - index_avg; // zero-average "time" for linear drifting of the azimuth and tilts.
				inv_new_xyzatr[1] = inv_new_atr.clone();
				for (int i = 0; i < omegas.length; i++) {
					inv_new_xyzatr[1][i] -= omegas[i] * time_drift;
				}
			}
			double [][] scene_xyzatr,dxyzatr_dt;
			String ts = quadCLTs[scene_index].getImageName();
			scene_xyzatr = ers_old_reference.getSceneXYZATR(ts);
			dxyzatr_dt =   ers_old_reference.getSceneErsXYZATR_dt(ts);
			double [][] scene_new_xyzatr = ErsCorrection.combineXYZATR(
					scene_xyzatr,    // double [][] reference_xyzatr,
					inv_new_xyzatr); // double [][] scene_xyzatr)
			ers_new_reference.addScene(
					ts,               // String timestamp,
					scene_new_xyzatr, // double [][] xyzatr,
					dxyzatr_dt);      // double [][] ers_xyzatr_dt)		
		}
    	return ref_new;
    }
    
    public static double [][] getRefPxPyD(
    		CLTParameters  clt_parameters,
    		boolean        mode_cuas,
    		Rectangle      fov_tiles,
    		double []      stereo_xyz, // offset reference camera {x,y,z}
    		double []      stereo_atr_in, // offset reference orientation (cuas)
    		double []      ref_disparity,			
    		QuadCLT        refCLT, // should be the same instance if one of quadCLTs
    		int            debugLevel) {
    	double [] stereo_atr =        (stereo_atr_in != null)? stereo_atr_in: OpticalFlow.ZERO3; // maybe later play with rotated camera
        double [][] ref_pXpYD;
        if (mode_cuas) { //  && (dbg_scene > 0)) {
			int         around =              2;
			double      around_sigma =        4.0;
			int         num_virtual_refines = 2;
			String      debugSuffix=         null; // "virtual";
			ref_pXpYD= transformFromVirtual(
					clt_parameters, // CLTParameters  clt_parameters,
					ref_disparity, // double []         disparity_ref,
					stereo_xyz, // final double []   scene_xyz,         // camera center in world (reference) coordinates
					stereo_atr, // final double []   scene_atr,         // camera orientation relative to world (reference) frame
					refCLT, // quadCLTs[ref_index], // final QuadCLT     reference_QuadClt,
					around, // final int         around,            // 2 search around for interpolation
					around_sigma, // final double      sigma,
					num_virtual_refines, // final int         num_refines,
					debugSuffix); // final String      debugSuffix)
			refCLT.getErsCorrection().setupERS();
			System.out.println("getRefPxPyD(): Calculated virtual_PxPyD");
        } else {
        	ref_pXpYD = OpticalFlow.transformToScenePxPyD( // now should work with offset ref_scene
        			fov_tiles,            // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
        			ref_disparity,        // final double []   disparity_ref, // invalid tiles - NaN in disparity
        			OpticalFlow.ZERO3, // stereo_xyz, // ZERO3,                // final double []   scene_xyz, // camera center in world coordinates
        			OpticalFlow.ZERO3, // stereo_atr, // ZERO3,                // final double []   scene_atr, // camera orientation relative to world frame
        			refCLT, // quadCLTs[ref_index],  // final QuadCLT     scene_QuadClt,
        			refCLT, // quadCLTs[ref_index],  // final QuadCLT     reference_QuadClt, // now - may be null - for testing if scene is rotated ref
        			QuadCLT.THREADS_MAX);          // int               threadsMax)
        }
        return ref_pXpYD;
    }

    /**
     * Re-sample disparity map to match uniform grid on the virtual view, such as
     * the view orthogonal to the terrain tangent/average surface 
     * @param clt_parameters configuration parameters
     * @param disparity_ref known disparity for the camera reference frame
     * @param scene_xyzatr virtual view center position and orientation relative to the reference frame
     * @param reference_QuadClt scene instance for the reference frame
     * @param around number of tiles to look around when interpolating using average planes
     * @param sigma reduce weight of far samples e-times at this distance
     * @param num_refines number of refine passes
     * @param debugSuffix image name suffix to generate debug images
     * @return array of per-tile disparity (NaN for undefined tiles) 
     */
    public static double [] getDisparityVirtual(
    		CLTParameters     clt_parameters,
		double []         disparity_ref,
		final double [][] scene_xyzatr,      // camera center and orientation in world (reference frame) coordinates
		final QuadCLT     reference_QuadClt,
		final int         around,            // 2 search around for interpolation
		final double      sigma,
		final int         num_refines,
		final String      debugSuffix){
			double [][] reference_pXpYD = 
					transformFromVirtual(
							clt_parameters, // CLTParameters     clt_parameters,
							disparity_ref,  // double []         disparity_ref,
							scene_xyzatr[0],      // final double []   scene_xyz,         // camera center in world (reference) coordinates
							scene_xyzatr[1],  // final double []   scene_atr,         // camera orientation relative to world (reference) frame
							reference_QuadClt, // final QuadCLT     reference_QuadClt,
							around,   // final int         around,            // 2 search around for interpolation
							 sigma, // final double      sigma,
							 num_refines,   // final int         num_refines,
							 debugSuffix); // final String      debugSuffix)
			// get scene pXpYD corresponding to reference_pXpYD
			double [][] scene_pXpYD = OpticalFlow.transformToScenePxPyD(
					reference_pXpYD,    // final double [][] reference_pXpYD,// invalid tiles - NaN in disparity. Should be no nulls, no NaN disparity
					scene_xyzatr[0],    // final double []   scene_xyz,         // camera center in world (reference) coordinates
					scene_xyzatr[1],    // final double []   scene_atr,         // camera orientation relative to world (reference) frame
					reference_QuadClt,  // final QuadCLT     reference_QuadClt) //
					null);              // final QuadCLT     scene_QuadClt) //
			double [] scene_disparity = new double [scene_pXpYD.length];
			Arrays.fill(scene_disparity, Double.NaN);
			for (int ntile = 0; ntile < scene_disparity.length; ntile++) if (scene_pXpYD[ntile] != null) {
				scene_disparity[ntile] = scene_pXpYD[ntile][2];
			}
			return scene_disparity; 
	
    }
    
    /**
     * Re-sample disparity map to match uniform grid on the virtual view, producing a non-uniform
     * coordinates+disparity in the reference scene that corresponds to the uniform grid in the virtual view.
     * @param clt_parameters configuration parameters
     * @param disparity_ref known disparity for the camera reference frame
     * @param scene_xyzatr virtual view center position and orientation relative to the reference frame
     * @param reference_QuadClt scene instance for the reference frame
     * @param around number of tiles to look around when interpolating using average planes
     * @param sigma reduce weight of far samples e-times at this distance
     * @param num_refines number of refine passes
     * @param debugSuffix image name suffix to generate debug images
     * @return array of non-uniform grid triplets of pixel X, pixel Y and disparity for the reference scene
     *         corresponding to the uniform grid in a virtual camera view 
     */

	public static double [][] transformFromVirtual(
			CLTParameters     clt_parameters,
			double []         disparity_ref,
			final double []   scene_xyz,         // camera center in world (reference) coordinates
			final double []   scene_atr,         // camera orientation relative to world (reference) frame
			final QuadCLT     reference_QuadClt,
			final int         around,            // 2 search around for interpolation
			final double      sigma,
			final int         num_refines,
			final String      debugSuffix){
		final boolean debug = (debugSuffix != null); //   && !clt_parameters.batch_run;
		final double sigma2 = 2*sigma*sigma;
    	final double normal_damping = 0.001; // pull to horizontal if not enough data 
    	final double [] damping = new double [] {normal_damping, normal_damping};
		TileProcessor tp =         reference_QuadClt.getTileProcessor();
		final int tilesX =         tp.getTilesX();
		final int tilesY =         tp.getTilesY();
		final int tiles =          tilesY*tilesX;
		final int tilesX_around =  tilesX + 2 * around; 
		final int tilesY_around =  tilesY + 2 * around; 
		final int tiles_around =   tilesY_around * tilesX_around;
		final int transform_size = tp.getTileSize();
		final Thread[] threads = ImageDtt.newThreadArray(ImageDtt.THREADS_MAX);
		final AtomicInteger ai = new AtomicInteger(0);
		int initial_capacity = 4;
		final int min_samples = 1;
		
		// fill NaN in disparity_ref (not needed for planes)
		boolean had_nans = false;
		for (int i = 0; i < disparity_ref.length; i++) {
			if (Double.isNaN(disparity_ref[i])) {
				had_nans=true;
				break;
			}
		}
		if (had_nans) {
			int grow_width = Math.max(tilesX, tilesY);
			int       num_passes = 20;

			System.out.println("transformFromVirtual(): disparity_ref had NaN, filling them from surrounding.");
			double [] disparity_ref1 = TileProcessor.fillNaNs(
					disparity_ref,  // final double [] data,
					null,            // final boolean [] prohibit,
					tilesX,       // int       width,
					2 * grow_width,  // 100, // 2*width, // 16,           // final int grow,
					0.7,             // double    diagonal_weight, // relative to ortho
					num_passes,      // int       num_passes,
					0.03);           // final double     max_rchange, //  = 0.01 - does not need to be accurate
			System.arraycopy(disparity_ref1, 0, disparity_ref, 0, disparity_ref.length);
		}
		
		final String debugTitle=debug?reference_QuadClt.getImageName()+"-"+debugSuffix : null;
		String [] debug_frame_titles = {"X", "Y", "D"};
		String [] debug_titles = new String[num_refines+1];
		for (int i = 0; i <= num_refines; i++) {
			debug_titles[i]=i+"";
		}
		final double [][][] debug_data = debug ? new double [3][num_refines+1][tiles]:null;
		if (debug) {
			for (int f = 0; f < debug_data.length; f++) {
				for (int i = 0; i < debug_data[f].length; i++) {
					Arrays.fill(debug_data[f][i], Double.NaN);
				}
			}
		}

		final ArrayList<ArrayList<Integer>> interp_list = new ArrayList<ArrayList<Integer>>(tiles_around);
		for (int nTile = 0; nTile < tiles_around; nTile++) {
			interp_list.add(new ArrayList<Integer>(initial_capacity));
		}
		/*
		 *  create uniform grid for initial interpolations
		 */  
		final double [][] reference_pXpYD = new double [tiles][];
		for (int nrefine = 0; nrefine <= num_refines; nrefine++) {
			if (nrefine == 0) {
				/*
				 * The first iteration of the reference_pXpYD is just a uniform grid with
				 * reference disparity
				 */
				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 (!Double.isNaN(disparity_ref[nTile])){
								double disparity = disparity_ref[nTile];
								int tileY = nTile / tilesX;  
								int tileX = nTile % tilesX;
								double centerX = (tileX + 0.5) * transform_size; //  - shiftX;
								double centerY = (tileY + 0.5) * transform_size; //  - shiftY;
								if (disparity < 0) {
									disparity = 1.0* disparity; // 0.0;
								}
								reference_pXpYD[nTile] = new double[] {centerX, centerY, disparity};
							}
						}
					};
				}		      
				ImageDtt.startAndJoin(threads);
		} else {
			/*
			 * Next iterations create uniform grid on the virtual scene  by the bi-linear
			 * interpolation using small patches around the grid points
			 * Start with creatring non-iniform grid scene_pXpYD, corresponding to the
			 * current reference_pXpYD (also non-uniform for all but the very first iteration)
			 * 
			 * tiles_around - larger grid    
			 */
			final double [][] scene_pXpYD = OpticalFlow.transformToScenePxPyD(
						reference_pXpYD,    // final double [][] reference_pXpYD,// invalid tiles - NaN in disparity. Should be no nulls, no NaN disparity
						scene_xyz,          // final double []   scene_xyz,         // camera center in world (reference) coordinates
						scene_atr,          // final double []   scene_atr,         // camera orientation relative to world (reference) frame
						reference_QuadClt, // final QuadCLT     reference_QuadClt) //
						null); // final QuadCLT     scene_QuadClt) //
				final double [][] reference_pXpYD_next = new double [tiles][];
				/*
				 * Initialize lists of the scene tiles containing converted reference tiles in the vicinity
				 * of scene uniform grid
				 */
				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_around; nTile = ai.getAndIncrement()) {
								interp_list.get(nTile).clear(); // clear each list
							}
						}
					};
				}		      
				ImageDtt.startAndJoin(threads);
				/*
				 * Populate lists in a single thread. Each reference_pXpYD tile projection scene_pXpYD
				 * gets to some uniform-grit tile. Some of the uniform grid tiles gets multiple reference
				 * sources, others - none.
				 */
				for (int nTile = 0; nTile < tiles; nTile++) if (scene_pXpYD[nTile] != null) {
					int tileX = (int) Math.floor (scene_pXpYD[nTile][0]/transform_size);
					int tileY = (int) Math.floor (scene_pXpYD[nTile][1]/transform_size);
					int tileX_around = tileX + around; 
					int tileY_around = tileY + around;
					if ((tileX_around >= 0) && (tileY_around >= 0) && (tileX_around < tilesX_around) && (tileY_around < tilesY_around)) {
						int tile_around = tileX_around + tilesX_around * tileY_around;
						interp_list.get(tile_around).add(nTile);
					}
				}
				/*
				 *  interpolate reference_pXpYD_next 
				 */
				ai.set(0);
				for (int ithread = 0; ithread < threads.length; ithread++) {
					threads[ithread] = new Thread() {
						public void run() {
							PolynomialApproximation pa = new PolynomialApproximation();		
							for (int nTile = ai.getAndIncrement(); nTile < tiles; nTile = ai.getAndIncrement()) {
								int tileY = nTile / tilesX;  
								int tileX = nTile % tilesX;
								double centerX = (tileX + 0.5) * transform_size; //  - shiftX;
								double centerY = (tileY + 0.5) * transform_size; //  - shiftY;
								int tileX_around = tileX + around; 
								int tileY_around = tileY + around;
								int num_samples = 0;
								for (int dy = -around; dy <= around; dy++) {
									for (int dx = -around; dx <= around; dx++) {
										int tile_around = (tileX_around + dx) + tilesX_around * (tileY_around + dy);
										num_samples+=interp_list.get(tile_around).size();
									}
								}
								int mindx = 0;
								if (num_samples >= min_samples) {
									/*
									 * inter/extrapolate with regularization (for few samples) and weights
									 */
									double [][][] mdata = new double [num_samples][3][];
									for (int dy = -around; dy <= around; dy++) {
										for (int dx = -around; dx <= around; dx++) {
											int tile_around = (tileX_around + dx) + tilesX_around * (tileY_around + dy);
											for (int nst:interp_list.get(tile_around)) {
												double [] ref_xyd = reference_pXpYD[nst];
												double [] scene_xyd = scene_pXpYD[nst];
												double x = scene_xyd[0]-centerX;
												double y = scene_xyd[1]-centerY;
												double w = Math.exp (-(x*x + y*y)/sigma2);
												mdata[mindx][0] = new double [2];
												mdata[mindx][0][0] =  x;
												mdata[mindx][0][1] =  y;
												mdata[mindx][1] = new double [2]; // [3];
												mdata[mindx][1][0] =  ref_xyd[0]; 
												mdata[mindx][1][1] =  ref_xyd[1]; 
//												mdata[mindx][1][2] =  ref_xyd[2];
												mdata[mindx][2] = new double [1];
												mdata[mindx][2][0] =  w;
												mindx++;
											}
										}
									}
									double[][] approx2d = pa.quadraticApproximation(
											mdata,
											true,       // boolean forceLinear,  // use linear approximation
											damping,    // double [] damping, null OK
											-1);        // debug level
									double px = approx2d[0][2];
									double py = approx2d[1][2];
									// interpolate disparity from disparity_ref
									double tx = px/transform_size-0.5;
									double ty = py/transform_size-0.5;
									int tx0 = (int) Math.floor(tx);
									int ty0 = (int) Math.floor(ty);
									int tx1 = tx0+1;
									int ty1 = ty0+1;
									double kx = tx-tx0;
									double ky = ty-ty0;
									if (tx0 < 0) {
										tx0 = 0;
										tx1 = tx0+1;
										kx = 0;
									} else if (tx1 >= tilesX) {
										tx1 = tilesX - 1;
										tx0 = tx1-1;
										kx = 1.0;
									}
									
									if (ty0 < 0) {
										ty0 = 0;
										ty1 = ty0+1;
										ky = 0;
									} else if (ty1 >= tilesY) {
										ty1 = tilesY - 1;
										ty0 = ty1-1;
										ky = 1.0;
									}
									double d = 
											(1-kx)*(1-ky)*disparity_ref[tx0 + tilesX*ty0] +
											(  kx)*(1-ky)*disparity_ref[tx1 + tilesX*ty0] +
											(1-kx)*(  ky)*disparity_ref[tx0 + tilesX*ty1] +
											(  kx)*(  ky)*disparity_ref[tx1 + tilesX*ty1];
									reference_pXpYD_next[nTile] = new double[] {px, py, d};
								}
							}
						}
					};
				}		      
				ImageDtt.startAndJoin(threads);
				System.arraycopy(
						reference_pXpYD_next,
						0,
						reference_pXpYD,
						0,
						tiles);
				// TODO: refine disparity from disparity_ref interpolation
			} // if (nrefine == 0) else
			if (debug) {
				for (int nTile = 0; nTile < tiles; nTile++) if (reference_pXpYD[nTile] != null) {
					debug_data[0][nrefine][nTile] = reference_pXpYD[nTile][0];
					debug_data[1][nrefine][nTile] = reference_pXpYD[nTile][1];
					debug_data[2][nrefine][nTile] = reference_pXpYD[nTile][2];
				}
			}
		}
		if (debug) {
			ImagePlus img_debug = ShowDoubleFloatArrays.showArraysHyperstack(
					debug_data,         // double[][][] pixels, 
					tilesX,             // int          width, 
					debugTitle,         // String       title, "time_derivs-rt"+diff_time_rt+"-rxy"+diff_time_rxy,
					debug_titles,       // String []    titles, // all slices*frames titles or just slice titles or null
					debug_frame_titles, // String []    frame_titles, // frame titles or null
					false); // true);              // boolean      show)
			reference_QuadClt.saveImagePlusInModelDirectory(img_debug);

		}		
		
		return reference_pXpYD; // ref_pXpYD_0; // 
	}

	/**
	 * Combine previous cumulative and current sequence data
	 * @param coherent       thisCuasData variants correspond to cumulative variants (CuasTile instances)
	 *                       and can be merged without looking for the best match (some new variants may be empty).
	 *                       If new variants list is longer than cumulative, it will be just added to the end. 
	 *                       If coherent=false, each new variant has to be matched with the best fit of the cumulative
	 *                       variant. 
	 * @param parentCuasData
	 * @param thisCuasData
	 * @param cuas_decay
	 * @param debug_title
	 * @param debugLevel
	 * @return
	 */
	public static CuasData combineAveragesCoherent(
			final CuasData    parentCuasData,
			final CuasData    thisCuasData,   // each tile list starts with those from the  parent_cuas
			final double      cuas_decay,    // seconds e times
			final double []   dbg_weights, // should be null or double [tilesX*tilesY] - will return debug weight ratio (new to cumul)   
			final int         debugLevel) {
		final int dbg_tile = 2029;
		final CuasData comboCuasData = parentCuasData.clone();
		final int num_tiles = parentCuasData.getNumTiles();
 		final Thread[] threads = ImageDtt.newThreadArray(ImageDtt.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 < num_tiles; nTile = ai.getAndIncrement()) {
						if (nTile == dbg_tile) {
							System.out.println("combineAveragesCoherent(): nTile="+nTile);
						}
						double w_new = 0, w_cumul = 0;
						ArrayList<CuasTile> this_tile_list =   thisCuasData.getTileData(nTile);	
						ArrayList<CuasTile> combo_tile_list =  comboCuasData.getTileData(nTile);	
						for (int i = 0; i < combo_tile_list.size();i++) { // merge matching
							if (dbg_weights != null) {
								w_new += this_tile_list.get(i).getWeight();
								w_cumul += combo_tile_list.get(i).getWeight(cuas_decay, this_tile_list.get(i).getTimeStamp());
							}
							combo_tile_list.get(i).merge(this_tile_list.get(i),cuas_decay);
						}
						for (int i = combo_tile_list.size(); i < this_tile_list.size(); i++) {
							combo_tile_list.add(this_tile_list.get(i));
							if (dbg_weights != null) {
								w_new += this_tile_list.get(i).getWeight();
							}
						}
						if (dbg_weights != null) {
							dbg_weights[nTile] = w_new / (w_new + w_cumul);
						}		
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return comboCuasData;
	}	

	public static CuasData combineAveragesNonCoherent(
			final CuasData    parentCuasData,
			final CuasData    thisCuasData,   // each tile list starts with those from the  parent_cuas
			final boolean     en_create, // create new variants
			final double      tolerance, // NaN works as infinity
			final double      cuas_decay,    // seconds e times
			final double      dts,            // only used for debug (when debug_title != null)
			final double []   dbg_weights, // should be null or double [tilesX*tilesY] - will return debug weight ratio (new to cumul)
			final int         debugLevel) {
		final int dbg_tile = 2029;
		final CuasData comboCuasData = parentCuasData.clone();
		final int num_tiles = parentCuasData.getNumTiles();
		final boolean all_variants = Double.isNaN(tolerance) || Double.isInfinite(tolerance);
		final double tolerance2 = tolerance*tolerance;
		final Thread[] threads = ImageDtt.newThreadArray(ImageDtt.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 < num_tiles; nTile = ai.getAndIncrement()) {
						if (nTile == dbg_tile) {
							System.out.println("combineAveragesNonCoherent(): nTile="+nTile);
						}
						ArrayList<CuasTile> this_tile_list =   thisCuasData.getTileData(nTile);	
						ArrayList<CuasTile> combo_tile_list =  comboCuasData.getTileData(nTile);
						if (dbg_weights != null) {
							double w_new = 0, w_cumul = 0;
							for (CuasTile tile : this_tile_list) {
								w_new += tile.getWeight(cuas_decay, dts);
							}
							for (CuasTile tile : combo_tile_list) { // before modified
								w_cumul += tile.getWeight(cuas_decay, dts);
							}
							dbg_weights[nTile] = w_new / (w_new + w_cumul);
							
						}
						for (CuasTile tile : this_tile_list) if (!tile.isEmpty()){
							double [] var_diffs2 = new double[combo_tile_list.size()]; // tile_list.size() >= tile_ref_list.size()
							double best_diff2 = Double.POSITIVE_INFINITY;
							int best_index = 0;
							for (int i = 0; i < var_diffs2.length; i++) {
								CuasTile ref_tile =combo_tile_list.get(i);
								if (!ref_tile.isEmpty()) {
									var_diffs2[i] = ref_tile.diffTile2(tile);
									if (var_diffs2[i] < best_diff2) {
										best_diff2 = var_diffs2[i];
										best_index = i;
									}
								} else {
									var_diffs2[i] = Double.NaN;
								}
							}
							if (all_variants || (best_diff2 <= tolerance2)) {
								combo_tile_list.get(best_index).merge(tile, cuas_decay);
							} else if (en_create) {
								combo_tile_list.add(tile);
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return comboCuasData;
	}	
	
	
	
	
	/**
	 * Fill gaps in thisCuasData from parentCuasData. Both have single-variant tiles (.size()<=1)
	 * @param parentCuasData
	 * @param thisCuasData
	 * @param debugLevel
	 * @return
	 */
	public static void fillNewGaps(
			final CuasData    parentCuasData,
			final CuasData    thisCuasData,   // each tile list starts with those from the  parent_cuas
			final int         debugLevel) {
		final Thread[] threads = ImageDtt.newThreadArray(ImageDtt.THREADS_MAX);
		final AtomicInteger ai = new AtomicInteger(0);
		final int num_tiles = parentCuasData.getNumTiles();
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) {
						ArrayList<CuasTile> this_tile_list =   thisCuasData.getTileData(nTile);	
						ArrayList<CuasTile> parent_tile_list =  parentCuasData.getTileData(nTile);
						if (this_tile_list.isEmpty()) {
							this_tile_list.addAll(parent_tile_list);
						} else if (parent_tile_list.get(0).getWeight()==0) {
							this_tile_list.get(0).merge(parent_tile_list.get(0),0);
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return;
	}	

	
	
	
	public static CuasData    filter(
			final CuasData   cuasData,
			final double     cuas_decay,    // seconds e times
			final double     keep_fraction,
			final int        debugLevel) {
	    final int dbg_tile = 2029;			
//		final CuasData filteredCuasData = cuasData.cloneEmpty();
	    // empty list for each tile
		final CuasData filteredCuasData = new CuasData(cuasData.num_colors, cuasData.width, cuasData.height);
		final Thread[] threads = ImageDtt.newThreadArray(ImageDtt.THREADS_MAX);
		final AtomicInteger ai = new AtomicInteger(0);
		final int num_tiles = cuasData.getNumTiles();
		final boolean stop_2 = false; //  true;
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) {
						ArrayList<CuasTile> tile_list =   cuasData.getTileData(nTile);	
						ArrayList<CuasTile> filtered_tile_list =  filteredCuasData.getTileData(nTile);	// empty
					    if (nTile == dbg_tile) {
					        System.out.println("filter(): nTile="+nTile);
					    }
					    if (stop_2 && (tile_list.size() > 1)) {
					        System.out.println("filter(): nTile="+nTile+", tile_list.size()="+tile_list.size());
					    }
					    
						if (!tile_list.isEmpty()) {
							if (tile_list.size() == 1) {// no variants, just copy
								filtered_tile_list.add(tile_list.get(0));
							} else {
								double dts = 0;
								double [] weights = new double[tile_list.size()];
								for (int i = 0; i < weights.length; i++) {
									if (tile_list.get(i).getTimeStamp() > dts) {
										dts = tile_list.get(i).getTimeStamp(); // find latest timestamp
									}
								}
								double sumw = 0;
								for (int i = 0; i < weights.length; i++) {
									CuasTile tile = tile_list.get(i);
									weights[i] =tile_list.get(i).getWeight(cuas_decay, dts);
									sumw+=weights[i];
								}
								for (int i = 0; i < weights.length; i++) {
									weights[i] /= sumw;
								}
								Integer [] indices = new Integer [weights.length];
								for (int i = 0; i < indices.length; i++) {
									indices[i] = i;
								}

								Arrays.sort(indices, new Comparator<Integer>() { // decreasing weight
									@Override
									public int compare(Integer lhs, Integer rhs) {
										return (weights[lhs] > weights[rhs]) ? -1 : (weights[lhs] < weights[rhs]) ? 1 : 0;
									}
								});
								double used_w = 0.0;
								for (int i = 0; i < indices.length; i++) {
									int indx = indices[i];
									filtered_tile_list.add(tile_list.get(indx));
									used_w += weights[indx];
									if (used_w >= keep_fraction) {
										break;
									}
								}
								
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return filteredCuasData;
	}	
	
}
