package com.elphel.imagej.cuas;

import java.awt.Rectangle;
import java.io.File;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.concurrent.atomic.AtomicInteger;

import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.common.PolynomialApproximation;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
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 ij.ImagePlus;

public class Cuas {
 
    public static QuadCLT createCenterClt( // assuming cuas_rotation is true
    		CLTParameters  clt_parameters,
    		QuadCLT []     quadCLTs,
    		QuadCLT        ref_scene, // 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;
    		int            debugLevel) {
    	QuadCLT last_clt = quadCLTs[quadCLTs.length-1]; // save center with the latest timestamp
    	if (range == null) {
    		range = new int [] {0, quadCLTs.length-1};
    	}
		double [][] center_ATR = CuasCenterLma.getCenterATR( // relative to ref_scene
				quadCLTs,    // QuadCLT [] quadCLTs,
				ref_scene,   // QuadCLT        ref_scene, //
				range,  //int     []     range,
				debugLevel); // int debugLevel);
		double [] cuas_xyz = new double[3]; //maybe use later
		double [] cuas_atr = new double [] { center_ATR[0][0], center_ATR[0][1], center_ATR[0][2]};
		double [][] cuas_xyzatr = {cuas_xyz, cuas_atr};
		String center_name = QuadCLT.getCenterDirName(quadCLTs[quadCLTs.length - 1].getImageName());
		
		QuadCLT center_CLT = changeReference(
				quadCLTs, // QuadCLT [] quadCLTs,
				ref_scene, // QuadCLT    ref_old,
	    		null, // QuadCLT    ref_new,
	    		center_name, // String     name_new, 
	    		cuas_xyzatr); // double[][] new_xyzatr);
		
		if (ref_combo_dsi == null) {
			ref_combo_dsi =ref_scene.restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky
		}
    	double [][] dls = { 
    			ref_combo_dsi[OpticalFlow.COMBO_DSN_INDX_DISP],
    			ref_combo_dsi[OpticalFlow.COMBO_DSN_INDX_LMA],
    			ref_combo_dsi[OpticalFlow.COMBO_DSN_INDX_STRENGTH]
    	};
    	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_scene,      // 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}
        		cuas_atr,          // double []      stereo_atr_in, // offset reference orientation (cuas)
        		ds[0],             // double []      ref_disparity,			
        		ref_scene,         // QuadCLT        refCLT, // should be the same instance if one of quadCLTs
        		debugLevel);       // int            debugLevel)
        //Filter ref_pXpYD from folds in pX, pY and other
        boolean show_debug = false;
        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_scene.getTilesX()*ref_scene.getTileSize();
        int height = ref_scene.getTilesY()*ref_scene.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 (show_debug) {
            	String debug_suffix="-center-disparity-pre-filtered";
            	center_CLT.show_pXpYD(
            			ref_pXpYD, // double [][] pXpYD,
            			debug_suffix, // String      suffix,
            			true);// boolean     show)
            }
        	ref_scene.windowPsPyD(	
        			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(center_CLT.getTilesX(), center_CLT.getTilesY());
        int       num_passes = 20;
        double [][] debug_disparity_center = show_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,
    			center_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 (show_debug) {
        	String debug_suffix="-center-disparity-filtered";
        	center_CLT.show_pXpYD(
        			ref_pXpYD, // double [][] pXpYD,
        			debug_suffix, // String      suffix,
        			true);// boolean     show)
        	ShowDoubleFloatArrays.showArrays(
        			debug_disparity_center,
        			center_CLT.getTilesX(),
        			center_CLT.getTilesY(),
        			true,
        			center_CLT.getImageName()+"-center-disparity-filtering",
        			new String[] {"before","after"});
        }
        
    	boolean save_weights = true; // always
    	double  cuas_clt_threshold = clt_parameters.imp.cuas_clt_threshold; // 20;
    	double  cuas_clt_decrease = clt_parameters.imp.cuas_clt_decrease; // 0.01;
		float [][][] center_clt_w = getTDComboSceneSequence(
				clt_parameters,      // CLTParameters  clt_parameters,
        		null, // ref_pXpYD,           // double [][]    ref_pXpYD,    		
				save_weights,        // boolean        save_weights, // output corresponding weights for each data
				true,                // boolean        merge_all,
				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
	    		null,                // float [][]     ref_slices, // same dimensions, compare to current
	    		cuas_clt_threshold,  // final double   clt_threshold, //  will be ignored as ref_slices== null
	    		cuas_clt_decrease,   // final double   clt_decrease,  //  will be ignored as ref_slices== null
	    		debugLevel);         // int            debugLevel)
		boolean test_diff = true; // false;
		if (test_diff || (cuas_clt_threshold>0)) {
			center_clt_w=getTDComboSceneSequence(
					clt_parameters,      // CLTParameters  clt_parameters,
	        		null, // ref_pXpYD,           // double [][]    ref_pXpYD,    		
					save_weights,        // boolean        save_weights, // output corresponding weights for each data
					true,                // boolean        merge_all,
					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
		    		center_clt_w[0],     // float [][]     ref_slices, // same dimensions, compare to current
		    		cuas_clt_threshold,  // final double   clt_threshold,
		    		cuas_clt_decrease,   // final double   clt_decrease,
		    		debugLevel);         // int            debugLevel)
		}
		
		center_CLT.setCenterClt( // only for merged sensors
				center_clt_w[0][0],  // float [] clt, // per CLT sample (4x pixels)
				center_clt_w[1][0]); // float [] clt_weights // per tile

		final int transform_size = ref_scene.getTileSize();
		final int tilesX =         ref_scene.getTilesX();
		final int tilesY =         ref_scene.getTilesY();
		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";
		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();

		return center_CLT;
    }
    
    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) {
        	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;
	}
	
      
    public static float [][][] getTDComboSceneSequence(
    		CLTParameters  clt_parameters,
    		double [][]    ref_pXpYD,    // TODO: Use disparity, ignore pXpYD?		
    		final boolean  save_weights, // output corresponding weights for each data
    		final boolean  merge_all,
    		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
    		float [][]     ref_slices, // same dimensions, compare to current
    		final double   clt_threshold,
    		final double   clt_decrease,
    		int            debugLevel) {


    	boolean debug_pxpyd = true; // false;
    	int dbg_slices = 3;
    	final int tilesX=refCLT.getTilesX();
    	final int tilesY=refCLT.getTilesX();
    	double[][] tile_diffs = (debug_pxpyd && (ref_slices != null)) ? 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);
    		}
    	}
    	
    	double [][][] dbg_PxPyD = debug_pxpyd? (new double [dbg_slices][quadCLTs.length][]):null;
    	double [][][] dbg_PxPyD_slice = debug_pxpyd? (new double [1][][]):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) {
        		refCLT.show_pXpYD(
        				ref_pXpYD, // double [][] pXpYD,
        				"-getTDComboSceneSequence", // String      suffix,
        				true); // boolean show)
        	}
        }
        
    	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 float [][] sumFclt = new float [num_slices][tile_length * num_tiles];
        final float [][] sum_weights = new float [num_slices][num_tiles];
        
		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);
			}
			String ts = quadCLTs[nscene].getImageName();
			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 = merge_all? -1: 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 = getVelocities(
//						quadCLTs, // QuadCLT []     quadCLTs,
//						nscene);  // int            nscene)
				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,
						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,
						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_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;
			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;
							double s2 = 0;
							int ns = num_slices * tile_length;
							boolean no_nans = true;
							check_nan: {
								for (int slice = 0; slice < num_slices; slice++) { // normally just one
									float [] fclt_slice =ffclt[slice];
//									float [] sumFclt_slice = sumFclt[slice];
									int indx = fclt_offs;
									for (int i = 0; i < tile_length; i++) {
										if (Float.isNaN(fclt_slice[indx++])) {
											no_nans = false;
											break check_nan;
										}
									}
								}			
							}
							if (!no_nans) {
								continue;
							}
							float w = 1.0f; // constant weight
							if ((ref_slices != null) && ((clt_threshold > 0) || (tile_diffs != null))) {
								for (int slice = 0; slice < num_slices; slice++) { // normally just one
									float [] fclt_slice = ffclt [slice];
									float [] fclt_ref =   ref_slices[slice];
									int indx =      fclt_offs;
									for (int i = 0; i < tile_length; i++) {
										double diff = fclt_slice[indx] - fclt_ref[indx];
										s2+= diff*diff;
										indx++;
									}										
								}
								double diff = Math.sqrt(s2/ns);
								if (diff > clt_threshold) {
									w = (float) clt_decrease;
								}
								if (tile_diffs != null) {
									tile_diffs[fnscene][nTile] = diff; 
								}
							}
							
							for (int slice = 0; slice < num_slices; slice++) { // normally just one
									int indx = fclt_offs;
									float [] fclt_slice =    ffclt[slice];
									float [] sumFclt_slice = sumFclt[slice];
									for (int i = 0; i < tile_length; i++) {
										sumFclt_slice[indx] += w * fclt_slice[indx];
										indx++;
									}
									sum_weights[slice][nTile] += w;
							}
						}
					}
				};
			}		      
			ImageDtt.startAndJoin(threads);
		}
		// calculate averages
		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_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;
			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
					true);              // boolean      show)
		}
		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";
        	ShowDoubleFloatArrays.showArrays(
        			tile_diffs,
        			tilesX,
        			tilesY,
        			true,
        			diff_title,
        			diff_titles);
		}
		
    	return 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
     * @return true if no errors
     */
    public static QuadCLT changeReference(
    		QuadCLT [] quadCLTs,
    		QuadCLT    ref_old,
    		QuadCLT    ref_new,
    		String     name_new, 
    		double[][] new_xyzatr) {
    	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();
		for (int scene_index = 0; scene_index < quadCLTs.length; scene_index++) {
			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(
					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;
    }

    
	public static double [][] transformFromVirtual(
			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;
		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;
		
		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) {
				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 {
				// get scene pXpYD corresponding to reference_pXpYD
				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) //

				// deep clone reference_pXpYD
				final double [][] reference_pXpYD_next = new double [tiles][];
				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);
				// not multithreaded 
				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) {
			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
					true);              // boolean      show)
			
		}		
		
		return reference_pXpYD; // ref_pXpYD_0; // 
	}
	
	
	
	

}
