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.gpu.GPUTileProcessor;
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;

public class Cuas {
    public static QuadCLT createCenterClt( // assuming cuas_rotation is true
    		CLTParameters  clt_parameters,
    		QuadCLT []     quadCLTs,
    		int            ref_index, // 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 ref_clt = quadCLTs[ref_index];
    	QuadCLT last_clt = quadCLTs[quadCLTs.length-1]; // save center with the latest timestamp
    	if (range == null) {
    		range = new int [] {0, quadCLTs.length-1};
    	}
    	if (ref_index <0) {
    		ref_index = quadCLTs.length-1;
    	}
		double [][] center_ATR = CuasCenterLma.getCenterATR(
				quadCLTs,    // QuadCLT [] quadCLTs,
				ref_index,   //int        ref_index,
				range,  //int     []     range,
				debugLevel); // int debugLevel);
		double [] cuas_atr = new double [] { center_ATR[0][0], center_ATR[0][1], center_ATR[0][2]};
		if (ref_combo_dsi == null) {
			ref_combo_dsi =quadCLTs[ref_index].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
        			quadCLTs[ref_index], // QuadCLT        scene,
        			debugLevel);			
		}
		// getRefPxPyD: pX, pY reference image X,Y correspomnding 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_clt,        // QuadCLT        refCLT, // should be the same instance if one of quadCLTs
        		debugLevel);    // int            debugLevel)
    	boolean save_weights = true; // always
		float [][] center_clt_w = getTDComboSceneSequence(
				clt_parameters,      // CLTParameters  clt_parameters,
        		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}
	    		cuas_atr,            // double []      stereo_atr_in, // offset reference orientation (cuas)
	    		null, // ds[0],      // double []      ref_disparity,	// may be null if ref_pXpYD != null	
	    		quadCLTs,            // QuadCLT []     quadCLTs,
	    		ref_clt,             // QuadCLT        refCLT, // should be the same instance if one of quadCLTs
	    		debugLevel);         // int            debugLevel)
		// remove weights
		float [][] center_clt = save_weights? new float [center_clt_w.length/2][]: center_clt_w;
		int [][] center_clt_num = save_weights? new int [center_clt_w.length/2][]: null;
		if (save_weights) { // remove second half (weights as integer numbers)
			for (int i = 0; i< center_clt.length; i++) {
				center_clt[i] = center_clt_w[i];
				center_clt_num[i] = new int [center_clt_w[i + center_clt.length].length];
				for (int j = 0; j < center_clt_num[i].length; j++) {
					center_clt_num[i][j] = (int) Math.round(center_clt_w[i + center_clt.length][j]);
				}
			}
		}
		int [] wh_c = ref_clt.getWHC(false);
		//final int tile_size_td = 4 * GPUTileProcessor.DTT_SIZE * GPUTileProcessor.DTT_SIZE;
		final int width_clt = wh_c[0] * 2; // to make image dimensions similar
		
		String center_name =  QuadCLT.getCenterDirName(last_clt.getImageName()); // make name from last timestamp, not reference
		String ref_dir_path = ref_clt.getX3dDirectory(center_name);
		File cdir = new File(ref_dir_path);
		QuadCLT center_CLT = new QuadCLT(ref_clt, center_name); //null
		cdir.mkdirs();
		center_CLT.setImagePath(cdir.getPath());
		center_CLT.setCenterClt(
				center_clt[0], // float [] clt,
				center_clt_num[0], // int []   clt_num,
				width_clt); // int      clt_width);
		TileProcessor tp =         quadCLTs[quadCLTs.length - 1].getTileProcessor();
		final int transform_size = tp.getTileSize();
		final int tilesX =         tp.getTilesX();
		final int tilesY =         tp.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);
		center_CLT.setDSIFromCombo(center_combo_dsi); // reformat
//		center_CLT.setDSI(center_combo_dsi); // WRONG!
		String rslt_suffix = "-INTER-INTRA";
		rslt_suffix += (clt_parameters.correlate_lma?"-LMA":"-NOLMA");
		/*
		// fixing NaN in strengths. It is uses to return RMS in Not needed - NaN was from Arrays.fill(combo_dsn_final[i], Double.NaN);
		// OpticalFlow:10348
		for (int slice: OpticalFlow.COMBO_DSN_NONNAN) { // new int[] {COMBO_DSN_INDX_STRENGTH,COMBO_DSN_INDX_STRENGTH_BG}) {
			if (center_combo_dsi[slice] != null) {
				for (int i = 0; i <center_combo_dsi[slice].length; i++) {
					if (Double.isNaN(center_combo_dsi[slice][i])) {
						center_combo_dsi[slice][i] = 0.0;
					}
				}
			}
		}
		*/

		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.saveDSI();
		center_CLT.saveCenterClt();

		
		/*
		boolean save_clt = true;
		boolean save_dsi = true;
		boolean save_in_ref = true;
		boolean save_in_last = true;
		if (save_clt) {
			final int height_clt = combo_seq_clt[0].length/width_clt;
			String [] clt_titles = new String [combo_seq_clt_w.length];
			for (int i = 0; i < combo_seq_clt.length; i++) {
				clt_titles[i] = "chn-"+i;
				if (save_weights) {
					clt_titles[i+combo_seq_clt.length] = "weight-"+i;
				}
			}
			String suffix_clt = "-clt_combo";
			if (save_weights) {
				suffix_clt+="_weights";
			}
			if (save_in_ref) {
			ref_clt.saveFloatArrayInModelDirectory( // error
					suffix_clt,     // String      suffix,
					clt_titles,       // combo_dsn_titles_full, // null,          // String []   labels, // or null
					combo_seq_clt_w,    // dbg_data,         // float [][] data,
					width_clt,        // int         width,
					height_clt);      // int         height)
			}
			if (save_in_last) {
				quadCLTs[quadCLTs.length - 1].saveFloatArrayInModelDirectory( // error
						suffix_clt,     // String      suffix,
						clt_titles,       // combo_dsn_titles_full, // null,          // String []   labels, // or null
						combo_seq_clt_w,    // dbg_data,         // float [][] data,
						width_clt,        // int         width,
						height_clt);      // int         height)
			}
		}
		
		if (save_dsi) {
//			final double [][] combo_dsi_cli = ref_clt.restoreComboDSI (true);
			TileProcessor tp =         quadCLTs[quadCLTs.length - 1].getTileProcessor();
			final int transform_size = tp.getTileSize();
			final int tilesX =         tp.getTilesX();
			final int tilesY =         tp.getTilesY();
			final double [][] center_dsi = interpolateDSI(
					ref_pXpYD,      // final double [][] ref_pXpYD,
					ref_dsi, // combo_dsi_cli,  // final double [][] dsi_in,
					transform_size, // final int         transform_size,
					tilesX,         // final int         tilesX,
					tilesY);        //final int         tilesY);
			
			String rslt_suffix = "-CENTER-INTER-INTRA";
			// use quadCLTs[quadCLTs.length - 1].restoreComboDSI ("-CENTER",true); to restore
			
			rslt_suffix += (clt_parameters.correlate_lma?"-LMA":"-NOLMA");
	    	if (save_in_ref) {
	    		ref_clt.saveDoubleArrayInModelDirectory( // error
	    				rslt_suffix,           // String      suffix,
	    				OpticalFlow.COMBO_DSN_TITLES, // combo_dsn_titles_full, // null,          // String []   labels, // or null
	    				center_dsi,       // dbg_data,         // double [][] data,
	    				tilesX,                // int         width,
	    				tilesY);               // int         height)
	    	}
	    	if (save_in_last) {
	    		quadCLTs[quadCLTs.length - 1].saveDoubleArrayInModelDirectory( // error
	    				rslt_suffix,           // String      suffix,
	    				OpticalFlow.COMBO_DSN_TITLES, // combo_dsn_titles_full, // null,          // String []   labels, // or null
	    				center_dsi,       // dbg_data,         // double [][] data,
	    				tilesX,                // int         width,
	    				tilesY);               // int         height)
	    	}
		}
		*/
		return center_CLT;
    }
	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( // never used
    		CLTParameters  clt_parameters,
    		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, // offset reference orientation (cuas)
    		double []      ref_disparity,			
    		QuadCLT []     quadCLTs,
    		QuadCLT        refCLT, // should be the same instance if one of quadCLTs
//    		int            ref_index,
    		int            debugLevel) {
//    	double [] stereo_atr =        (stereo_atr_in != null)? stereo_atr_in: ZERO3; // maybe later play with rotated camera
    	boolean mode_cuas =   (stereo_atr!=null) && ((stereo_atr[0] != 0) || (stereo_atr[1] != 0) || (stereo_atr[2] != 0));
        double [][] ref_pXpYD = getRefPxPyD(
        		clt_parameters, // CLTParameters  clt_parameters,
        		mode_cuas,      // boolean        mode_cuas,            		
//        		save_weights,   // final boolean  save_weights, // output corresponding weights for each data
        		fov_tiles,      // Rectangle      fov_tiles,
        		stereo_xyz,     // double []      stereo_xyz, // offset reference camera {x,y,z}
        		stereo_atr,     // double []      stereo_atr_in, // offset reference orientation (cuas)
        		ref_disparity,  // double []      ref_disparity,			
        		refCLT,         // QuadCLT        refCLT, // should be the same instance if one of quadCLTs
        		debugLevel);    // int            debugLevel)
        
        return getTDComboSceneSequence(
        		clt_parameters, // CLTParameters  clt_parameters,
        		ref_pXpYD,      // double [][]    ref_pXpYD,    		
        		save_weights,   // final boolean  save_weights, // output corresponding weights for each data
        		merge_all,      // final boolean  merge_all,
        		sensor_mask,    // final int      sensor_mask,
        		fov_tiles,      // Rectangle      fov_tiles,
        		stereo_xyz,     // double []      stereo_xyz, // offset reference camera {x,y,z}
        		stereo_atr,     // double []      stereo_atr_in, // offset reference orientation (cuas)
        		ref_disparity,  // double []      ref_disparity,			
        		quadCLTs,       // QuadCLT []     quadCLTs,
        		refCLT,         // QuadCLT        refCLT, // should be the same instance if one of quadCLTs
        		debugLevel);    // int            debugLevel);
    }
 	
    public static float [][] getTDComboSceneSequence(
    		CLTParameters  clt_parameters,
    		double [][]    ref_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,			
    		QuadCLT []     quadCLTs,
    		QuadCLT        refCLT, // should be the same instance if one of quadCLTs
    		int            debugLevel) {
        int dbg_scene = -95;
    	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 = 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
		ErsCorrection ers_reference = refCLT.getErsCorrection();
        //  quadCLTs[0].getCltLength(boolean use_ref)
        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 float [][] sumFclt = new float [num_slices * (save_weights? 2:1)][quadCLTs[sc0].getCltSize(false)];
        final int [][] numAcc = new int [num_slices][sumFclt[0].length];
        // next two to improve multithreading performance
        final int tile_size_td = 4 * GPUTileProcessor.DTT_SIZE * GPUTileProcessor.DTT_SIZE;
        final int tiles_td = sumFclt[0].length/tile_size_td;
        final int tiles_td_all = tiles_td * num_slices; // usually sumFclt.length==1
        
		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){
			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 (nscene != ref_index) { // Check even for raw, so video frames will match in all modes 
			if (quadCLTs[nscene] != refCLT) { // Check even for raw, so video frames will match in all modes 
				scene_xyz = ers_reference.getSceneXYZ(ts);
				scene_atr = ers_reference.getSceneATR(ts);
				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,
						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,
						QuadCLT.THREADS_MAX, // int               threadsMax,
						debugLevel);         // final int         debugLevel) 

			} else {
				fclt = QuadCLT.getTDCombo(
						sm,                  // final int         sensor_mask,
						merge_all,           // final boolean     merge_channels,
						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,
						QuadCLT.THREADS_MAX, // int               threadsMax,
						debugLevel);         // final int         debugLevel) 
			}
			final float [][] ffclt = fclt;
			ai.set(0);
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					public void run() {
						for (int nTileAll = ai.getAndIncrement(); nTileAll < tiles_td_all; nTileAll = ai.getAndIncrement()) {
							int ntile = nTileAll % tiles_td;
							int nsens = nTileAll / tiles_td;
							int indx0 = ntile * tile_size_td;
							int indx1 = indx0 + tile_size_td;
							for (int indx = indx0; indx < indx1; indx++) {
								float d =ffclt[nsens][indx];
								if (!Float.isNaN(d)){
									sumFclt[nsens][indx] += d;
									numAcc[nsens][indx]++ ;
								}
							}
						}
					}
				};
			}		      
			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 nTileAll = ai.getAndIncrement(); nTileAll < tiles_td_all; nTileAll = ai.getAndIncrement()) {
						int ntile = nTileAll % tiles_td;
						int nsens = nTileAll / tiles_td;
						int indx0 = ntile * tile_size_td;
						int indx1 = indx0 + tile_size_td;
						for (int indx = indx0; indx < indx1; indx++) {
							if (numAcc[nsens][indx] > 0) {
								sumFclt[nsens][indx]/=numAcc[nsens][indx];
							} else {
								sumFclt[nsens][indx] = Float.NaN;
							}
							if (save_weights) {
								sumFclt[nsens + num_slices][indx] = numAcc[nsens][indx];
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
    	return sumFclt;
    }
    
    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; // 
	}

}
