/**
 **
 ** StructureFromMotion - calculate depth from scene sequences
 **
 ** Copyright (C) 2023 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  StructureFromMotion.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.tileprocessor.sfm;

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

import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.gpu.GPUTileProcessor;
//import com.elphel.imagej.cameras.ColorProcParameters;
//import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.gpu.GpuQuad;
import com.elphel.imagej.gpu.TpTask;
//import com.elphel.imagej.ims.Did_ins_2;
//import com.elphel.imagej.ims.Imx5;
import com.elphel.imagej.tileprocessor.Correlation2d;
import com.elphel.imagej.tileprocessor.ErsCorrection;
import com.elphel.imagej.tileprocessor.ImageDtt;
import com.elphel.imagej.tileprocessor.Interscene;
import com.elphel.imagej.tileprocessor.IntersceneLma;
import com.elphel.imagej.tileprocessor.OpticalFlow;
import com.elphel.imagej.tileprocessor.QuadCLT;
import com.elphel.imagej.tileprocessor.TDCorrTile;

import ij.ImagePlus;

public class StructureFromMotion {
	public static double [] ZERO3 = {0.0,0.0,0.0};
	public static int     THREADS_MAX =          100;  // maximal number of threads to launch
//	class SfmCorr{
//		double sfm_gain;
//		double [] corr_ind;  // {disparity, strength}
//		double [] corr_neib; // {disparity, strength}
//	}

	public static boolean sfmPair_ref_debug( // ref/scene
			final CLTParameters          clt_parameters,
			final QuadCLT                ref_scene,
			final QuadCLT                scene,
			double                       mb_max_gain,
			final boolean                batch_mode,
			final int                    debugLevel) {
		double      range_disparity_offset = clt_parameters.imp.range_disparity_offset;
		boolean mb_en =       clt_parameters.imp.mb_en;
		double  mb_tau =      clt_parameters.imp.mb_tau;      // 0.008; // time constant, sec
		int     margin =      clt_parameters.imp.margin;
		int        sensor_mask_inter = clt_parameters.imp.sensor_mask_inter ; //-1;
		int tilesX =    ref_scene.getTileProcessor().getTilesX();
		int tilesY =    ref_scene.getTileProcessor().getTilesY();
		ErsCorrection ers_reference = ref_scene.getErsCorrection();		
		boolean [] reliable_ref =  null;
		boolean use_combo_dsi =    true;
		boolean use_lma_dsi =      clt_parameters.imp.use_lma_dsi;
		double [][] ref_xyzatr_dt= {
				ers_reference.getErsXYZ_dt(),
				ers_reference.getErsATR_dt()
		};
		for (int i = 0; i < ref_xyzatr_dt.length; i++) {
			if (ref_xyzatr_dt[i] == null) {
				System.out.println("sfmPair(): ref_xyzatr_dt["+i+"] == null");
				ref_xyzatr_dt[i] = ZERO3.clone();
			}
		}
		

        double [] disparity_raw = new double [tilesX * tilesY];
        Arrays.fill(disparity_raw,clt_parameters.disparity);
        double [][] combo_dsn_final = ref_scene.restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky

        double [][] dls = { // Update to use FG? Or FG/no BG?
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP],
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA],
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_STRENGTH]
        };
        double [][] ds = OpticalFlow.conditionInitialDS(
        		true,                // boolean        use_conf,       // use configuration parameters, false - use following  
        		clt_parameters,      // CLTParameters  clt_parameters,
        		dls,                 // double [][]    dls
        		ref_scene,           // QuadCLT        scene,
        		debugLevel);			
        double [] interscene_ref_disparity = null; // keep null to use old single-scene disparity for interscene matching
        if (use_combo_dsi) {
        	interscene_ref_disparity = ds[0].clone(); // use_lma_dsi ?
        	if (use_lma_dsi) {
        		for (int i = 0; i < interscene_ref_disparity.length; i++) {
        			if (Double.isNaN(dls[1][i])) {
        				interscene_ref_disparity[i] = Double.NaN;
        			}
        		}
        	}
        }

        double [][] ref_pXpYD = null;
        double [] mb_ref_disparity =null;
        mb_ref_disparity = interscene_ref_disparity;
        if (mb_ref_disparity == null) {
        	mb_ref_disparity =ref_scene.getDLS()[use_lma_dsi?1:0];
        }
        // ref_pXpYD should correspond to uniform grid (do not undo ERS of the reference scene)
        ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
        		null, // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
        		mb_ref_disparity, // dls[0],  // final double []   disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
        		ZERO3,         // final double []   scene_xyz, // camera center in world coordinates
        		ZERO3,         // final double []   scene_atr, // camera orientation relative to world frame
        		ref_scene,     // final QuadCLT     scene_QuadClt,
        		ref_scene);    // final QuadCLT     reference_QuadClt)

		// should have at least next or previous non-null
		double [][] mb_vectors_ref = null;
		TpTask[][] tp_tasks_ref = null;
		String ts = scene.getImageName();
		if ((ers_reference.getSceneXYZ(ts)== null) || (ers_reference.getSceneATR(ts)== null)) {
			System.out.println("sfmPair(): no pose for timestamp "+ts);
			return false;
		}
		double [][]	scene_xyzatr = new double[][] {ers_reference.getSceneXYZ(ts), ers_reference.getSceneATR(ts)};
		double [][] scene_xyzatr_dt= {
				ers_reference.getSceneErsXYZ_dt(ts),
				ers_reference.getSceneErsATR_dt(ts)
		};
		for (int i = 0; i < scene_xyzatr_dt.length; i++) {
			if (scene_xyzatr_dt[i] == null) {
				System.out.println("sfmPair(): scene_xyzatr_dt["+i+"] == null");
				scene_xyzatr_dt[i] = ZERO3.clone();
			}
		}
		double [][]        mb_vectors = null;
		
		if (mb_en) {
			// should get velocities from HashMap at reference scene from timestamp , not re-calculate.
			//				double [][] dxyzatr_dt_ref = ref_xyzatr_dt;
			mb_vectors_ref = OpticalFlow.getMotionBlur(
					ref_scene,             // QuadCLT        ref_scene,
					ref_scene,             // QuadCLT        scene,         // can be the same as ref_scene
					ref_pXpYD,             // double [][]    ref_pXpYD,     // here it is scene, not reference!
					ZERO3,                 // double []      camera_xyz,
					ZERO3,                 // double []      camera_atr,
					ref_xyzatr_dt[0],      // double []      camera_xyz_dt,
					ref_xyzatr_dt[1],      // double []      camera_atr_dt,
					0,                     // int            shrink_gaps,  // will gaps, but not more that grow by this
					debugLevel);           // int            debug_level)
		}
		tp_tasks_ref = Interscene.setReferenceGPU (
				clt_parameters,            // CLTParameters      clt_parameters,			
				ref_scene,                 // QuadCLT            ref_scene,
				mb_ref_disparity,          // double []          ref_disparity, // null or alternative reference disparity
				ref_pXpYD,                 // double [][]        ref_pXpYD,
				reliable_ref,              // final boolean []   selection, // may be null, if not null do not  process unselected tiles
				margin,                    // final int          margin,
				// motion blur compensation 
				mb_tau,                    // double             mb_tau,      // 0.008; // time constant, sec
				mb_max_gain,               // double             mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
				mb_vectors_ref,            // double [][]        mb_vectors,  // now [2][ntiles];
				debugLevel);               // int         debugLevel)
		if (mb_en) {
			mb_vectors = OpticalFlow.getMotionBlur(
					ref_scene,             // QuadCLT        ref_scene,
					scene,                 // QuadCLT        scene,         // can be the same as ref_scene
					ref_pXpYD,             // double [][]    ref_pXpYD,     // here it is scene, not reference!
					scene_xyzatr[0],       // double []      camera_xyz,
					scene_xyzatr[1],       // double []      camera_atr,
					scene_xyzatr_dt[0],    // double []      camera_xyz_dt,
					scene_xyzatr_dt[1],    // double []      camera_atr_dt,
					0,                     // int            shrink_gaps,  // will gaps, but not more that grow by this
					debugLevel);           // int            debug_level)
		}
		int [] fail_reason = new int[1];
		double [] ref_disparity = null;
		float [][][] facc_2d_img = new float [1][][]; // set it to null?
		// show debug image
    	@SuppressWarnings("unused")
		double [][] dpXYddisp = getSfmDpxDpyDdisp(
    			new double[][][] {new double[2][3], scene_xyzatr},// final double [][][] scenes_xyzatr,
    			ref_scene,              // scene_pairs[0][0], // final QuadCLT       scene0,
    			scene,                  // scene_pairs[0][1], // final QuadCLT       scene1,
    			ref_scene,              // final QuadCLT       ref_QuadClt,
    			ref_pXpYD,              // final double [][]   ref_pXpYD, // centers
    			range_disparity_offset, // final double        range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ;
    			batch_mode,             // final boolean       batch_mode,
    			debugLevel);            // final int           debug_level
		
		
		@SuppressWarnings("unused")
		double [][][] coord_motion = Interscene.interCorrPair( // new double [tilesY][tilesX][][];
				clt_parameters,      // CLTParameters  clt_parameters,
				false,               // use3D,               // boolean        use3D,         // generate disparity difference
				true,                // fpn_disable,         //	boolean        fpn_disable,   // disable fpn filter if images are known to be too close
				mb_max_gain,         // double         mb_max_gain,					
				null,                // min_max,             // double []      min_max,       // null or pair of minimal and maximal offsets
				fail_reason,         // int []         fail_reason,   // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
				ref_scene,           // QuadCLT        reference_QuadCLT,
				ref_disparity,       // double []      ref_disparity, // null or alternative reference disparity
				ref_pXpYD,           // double [][]    pXpYD_ref,     // pXpYD for the reference scene			
				tp_tasks_ref[0],     // TpTask[]       tp_tasks_ref,  // only (main if MB correction) tasks for FPN correction
				scene,               // QuadCLT        scene_QuadCLT,
				scene_xyzatr[0],     //                xyz
				scene_xyzatr[1],     // pose[1],      // atr
				reliable_ref,        // ****null,     // final boolean [] selection, // may be null, if not null do not  process unselected tiles
				margin,              // final int      margin,
				sensor_mask_inter,   // final int      sensor_mask_inter, // The bitmask - which sensors to correlate, -1 - all.
				facc_2d_img,         // final float [][][]   accum_2d_corr, // if [1][][] - return accumulated 2d correlations (all pairs)final float [][][]   accum_2d_corr, // if [1][][] - return accumulated 2d correlations (all pairs)
				null,                //	final float [][] dbg_corr_fpn,
				true,                // near_important,      // boolean            near_important, // do not reduce weight of the near tiles
				false,               // boolean            all_fpn,        // do not lower thresholds for non-fpn (used during search)
				false,               // initial_adjust,      // boolean             initial_adjust,
				mb_vectors,          // double [][]        mb_vectors,  // now [2][ntiles];
				clt_parameters.imp.debug_level, // int                imp_debug_level,
				debugLevel); // 1); // -1); // int debug_level);
		return true;
	}

	public static double [][] sfmPair_debug( // debug scene pairs
			final CLTParameters          clt_parameters,
			final QuadCLT                ref_scene,
			final QuadCLT [][]           scene_pairs,
			double                       mb_max_gain,
			final boolean                batch_mode,
			final int                    debugLevel) {
		boolean        show_2d_correlations = false; // true;
		boolean        show_disp_corr =       false; //true;
		boolean        show_disp_seq =        true;
		int            num_readjust =         5;
		double min_strength =                 0.4;
		double      range_disparity_offset = clt_parameters.imp.range_disparity_offset;
		// 16 puts scale same as with older code
		double corr_fz_inter = 16* clt_parameters.getGpuFatZeroInter(ref_scene.isMonochrome());	
		final int               corr_size = 2 * GPUTileProcessor.DTT_SIZE -1;
		double []  neib_weights_od = {0.7, 0.5};
		int tilesX =    ref_scene.getTileProcessor().getTilesX();
		int tilesY =    ref_scene.getTileProcessor().getTilesY();
//        int tile_size = ref_scene.getTileProcessor().getTileSize();
		ErsCorrection ers_reference = ref_scene.getErsCorrection();		
//		boolean [] reliable_ref =  null;
		boolean use_combo_dsi =    true;
		boolean use_lma_dsi =      clt_parameters.imp.use_lma_dsi;
		double [][] ref_xyzatr =    new double [][] {ZERO3,ZERO3};
		double [][] ref_xyzatr_dt= {
				ers_reference.getErsXYZ_dt(),
				ers_reference.getErsATR_dt()
		};
		for (int i = 0; i < ref_xyzatr_dt.length; i++) {
			if (ref_xyzatr_dt[i] == null) {
				System.out.println("sfmPair(): ref_xyzatr_dt["+i+"] == null");
				ref_xyzatr_dt[i] = ZERO3.clone();
			}
		}

        double [] disparity_raw = new double [tilesX * tilesY];
        Arrays.fill(disparity_raw,clt_parameters.disparity);
        double [][] combo_dsn_final = ref_scene.restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky

        double [][] dls = { // Update to use FG? Or FG/no BG?
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP],
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA],
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_STRENGTH]
        };
        double [][] ds = OpticalFlow.conditionInitialDS(
        		true,                // boolean        use_conf,       // use configuration parameters, false - use following  
        		clt_parameters,      // CLTParameters  clt_parameters,
        		dls,                 // double [][]    dls
        		ref_scene,           // QuadCLT        scene,
        		debugLevel);			
        double [] interscene_ref_disparity = null; // keep null to use old single-scene disparity for interscene matching
        if (use_combo_dsi) {
        	interscene_ref_disparity = ds[0].clone(); // use_lma_dsi ?
        	if (use_lma_dsi) {
        		for (int i = 0; i < interscene_ref_disparity.length; i++) {
        			if (Double.isNaN(dls[1][i])) {
        				interscene_ref_disparity[i] = Double.NaN;
        			}
        		}
        	}
        }
        
        double []   ref_disparity = interscene_ref_disparity;
        if (ref_disparity == null) {
        	ref_disparity =ref_scene.getDLS()[use_lma_dsi?1:0];
        }
        double [][]disp_adj = new double [num_readjust+1][];
        disp_adj[0] = ref_disparity.clone();
    	final int num_pairs = scene_pairs.length;
        for (int ntry = 0; ntry < num_readjust; ntry++) {
        	// only for derivatives
        	double [][] ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
        			null, // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
        			ref_disparity, // dls[0],  // final double []   disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
        			ZERO3,         // final double []   scene_xyz, // camera center in world coordinates
        			ZERO3,         // final double []   scene_atr, // camera orientation relative to world frame
        			ref_scene,     // final QuadCLT     scene_QuadClt,
        			ref_scene);    // final QuadCLT     reference_QuadClt)
        	TDCorrTile [][] pairs_TD_all = new TDCorrTile [2*(scene_pairs.length + 1)][];
        	final double[][][][] scenes_xyzatr = new double[num_pairs][2][][];   // 2 scenes
        	final double[][][][] scenes_xyzatr_dt  = new double[num_pairs][2][][];// 2 scenes
        	for (int npair = 0; npair < num_pairs; npair++) {
        		QuadCLT [] scenes = scene_pairs[npair];
        		// should have at least next or previous non-null
        		//        	final double[][][] scenes_xyzatr = new double[scenes.length][][];   // 2 scenes
        		//        	final double[][][] scenes_xyzatr_dt  = new double[scenes.length][][];// 2 scenes
        		for (int nscene = 0; nscene < scenes.length; nscene++) {
        			String ts = scenes[nscene].getImageName();
        			if ((ers_reference.getSceneXYZ(ts)== null) || (ers_reference.getSceneATR(ts)== null)) {
        				System.out.println("sfmPair(): no pose for timestamp "+ts);
        				return null;
        			}
        			scenes_xyzatr[npair][nscene] = new double[][] {
        				ers_reference.getSceneXYZ(ts),
        				ers_reference.getSceneATR(ts)};
        				scenes_xyzatr_dt[npair][nscene]= new double[][] {
        					ers_reference.getSceneErsXYZ_dt(ts),
        					ers_reference.getSceneErsATR_dt(ts)};
        					for (int i = 0; i < scenes_xyzatr_dt[nscene].length; i++) {
        						if (scenes_xyzatr_dt[npair][nscene][i] == null) {
        							System.out.println("sfmPair(): scene_xyzatr_dt["+i+"] == null");
        							scenes_xyzatr_dt[npair][nscene][i] = ZERO3.clone();
        						}
        					}
        		}
        	}        


        	double [][] dpXYddisp = getSfmDpxDpyDdisp(
        			scenes_xyzatr[0],          // final double [][][] scenes_xyzatr,
        			scene_pairs[0][0], // final QuadCLT       scene0,
        			scene_pairs[0][1], // final QuadCLT       scene1,
        			ref_scene,              // final QuadCLT       ref_QuadClt,
        			ref_pXpYD,              // final double [][]   ref_pXpYD, // centers
        			range_disparity_offset, // final double        range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ;
        			batch_mode,             // final boolean       batch_mode,
        			debugLevel);            // final int           debug_level

        	for (int npair = 0; npair < num_pairs; npair++) {
        		QuadCLT [] scenes = scene_pairs[npair];
        		pairs_TD_all[npair] = sfmPair_TD( // scene0/scene1
        				clt_parameters,  // final CLTParameters          clt_parameters,
        				ref_scene,       // final QuadCLT                ref_scene,
        				ref_disparity,   // final double []              ref_disparity, // here can not be null
        				ref_xyzatr,      // final double [][]            ref_xyzatr, // {ZERO3,ZERO3};
        				ref_xyzatr_dt,   // final double [][]            ref_xyzatr_dt,
        				scenes,          // final QuadCLT []             scenes,          // 2 scenes
        				scenes_xyzatr[npair],   //final double[][][]	         scenes_xyzatr,   // 2 scenes
        				scenes_xyzatr_dt[npair],// final double[][][]	         scenes_xyzatr_dt,// 2 scenes
        				mb_max_gain,     // double                       mb_max_gain,
        				batch_mode,      // final boolean                batch_mode,
        				debugLevel);     // final int                    debugLevel);
        		// get neibs in TD
        	}
        	// accumulate all pairs
        	pairs_TD_all[num_pairs] = TDCorrTile.cloneTiles(pairs_TD_all[0]);
        	for (int npair = 1; npair < num_pairs; npair++) {
        		TDCorrTile.accumulate (
        				pairs_TD_all[num_pairs], // final TDCorrTile [] dst,
        				pairs_TD_all[npair],     // final TDCorrTile [] src,
        				1.0);                    // final double src_weight);
        	}
        	// create neighbors for individual and combo
        	for (int npair = 0; npair <= num_pairs; npair++) {
        		pairs_TD_all[num_pairs + 1 + npair] = TDCorrTile.calcNeibs(
        				pairs_TD_all[npair], // pairs_TD,        // TDCorrTile [] tiles,
        				tilesX,              // final int     tilesX,
        				neib_weights_od,     //double []     neib_weights_od, // {orhto, diag}
        				true);               // final boolean process_all	
        	}

        	double [][][] corr2d_PD = new double [pairs_TD_all.length][][];
        	for (int npair = 0; npair < pairs_TD_all.length; npair++) {
        		corr2d_PD[npair] = 	TDCorrTile.convertTDtoPD(
        				ref_scene.getGPU(), // final GpuQuad       gpuQuad,
        				pairs_TD_all[npair],           // final TDCorrTile [] tiles,
        				0xFE,               // final int           corr_type, // 0xFE
        				corr_fz_inter,      // final double        gpu_fat_zero,
        				debugLevel);        // final int           debug_level

        	}
        	//		
        	if (show_2d_correlations) {
        		double [][] dbg_2d_corrs = ImageDtt.corr_partial_dbg( // not used in lwir
        				corr2d_PD,                           // final double [][][]     corr_data,       // [layer][tile][(2*transform_size-1)*(2*transform_size-1)] // if null - will not calculate
        				tilesX,                              // final int               tilesX,
        				corr_size,                           //final int               corr_size,       // 15
        				clt_parameters.corr_border_contrast, // final double            border_contrast,
        				debugLevel);                         // final int               globalDebugLevel)
        		String [] dbg_titles= new String[dbg_2d_corrs.length];
        		for (int npair = 0; npair < num_pairs; npair++) {
        			dbg_titles[npair] = "single-"+npair;
        			dbg_titles[npair+num_pairs+1] = "neibs-"+npair;
        		}
        		dbg_titles[num_pairs] = "single-avg"+num_pairs;
        		dbg_titles[2 * num_pairs + 1] = "neibs-avg"+num_pairs;

        		ShowDoubleFloatArrays.showArrays(
        				dbg_2d_corrs,
        				tilesX * (corr_size + 1),
        				tilesY * (corr_size + 1),
        				true,
        				"corr2d-"+scene_pairs[num_pairs-1][1].getImageName()+"-"+
        						scene_pairs[0][0].getImageName()+"-"+num_pairs,
        						dbg_titles);
        	}
        	double [][] corr2d_PD_use = corr2d_PD[num_pairs];
        	final double centroid_radius = clt_parameters.imp.centroid_radius; // final double              centroid_radius, // 0 - use all tile, >0 - cosine window around local max
        	final int    n_recenter = clt_parameters.imp.n_recenter;           // when cosine window, re-center window this many times
        	double [][] disp_corr = getSfmDisparityCorrectionStrength(
        			clt_parameters,  // final CLTParameters  clt_parameters,
        			corr2d_PD_use,   // final double [][]    corr2d_PD,
        			dpXYddisp,       // final double [][]    dpXYddisp,
        			centroid_radius, // final double         centroid_radius, // 0 - use all tile, >0 - cosine window around local max
        			n_recenter,      // final int            n_recenter,      // when cosine window, re-center window this many times
        			corr_size,       //final int            corr_size, // 15
        			debugLevel);     // final int            debugLevel);
        	for (int nTile = 0; nTile< disp_corr.length; nTile++) if (disp_corr[nTile] != null){
        		if (disp_corr[nTile][1] > min_strength) {
        			ref_disparity[nTile] += disp_corr[nTile][0];
        		}
        	}
        	// replace weak tiles with average?
        	disp_adj[ntry + 1] = ref_disparity.clone();
        	if (show_disp_corr) {
        		double [][] dbg_2d_corr = new double [3][disp_corr.length];
        		for (int i = 0; i < dbg_2d_corr.length; i++) {
        			Arrays.fill(dbg_2d_corr[i], Double.NaN);
        		}
        		for (int nTile = 0; nTile < disp_corr.length; nTile++)  if (disp_corr[nTile] != null) {
        			dbg_2d_corr[0][nTile] = disp_corr[nTile][0];
        			dbg_2d_corr[1][nTile] = disp_corr[nTile][1];
        		}
        		dbg_2d_corr[2] = ref_disparity.clone();
        		String [] dbg_titles = {"disp_corr","strength", "new_disp"};
        		ShowDoubleFloatArrays.showArrays(
        				dbg_2d_corr,
        				tilesX,
        				tilesY,
        				true,
        				"disp_corr-"+scene_pairs[num_pairs-1][1].getImageName()+"-"+
        						scene_pairs[0][0].getImageName()+"-"+num_pairs,
        						dbg_titles);
        	}
        	
        } // ntry
    	if (show_disp_seq) {
//    		String [] dbg_titles = new String[disp_adj.length]; // {"disp_corr","strength", "new_disp"};
    		ShowDoubleFloatArrays.showArrays(
    				disp_adj,
    				tilesX,
    				tilesY,
    				true,
    				"SfM-disparity-"+scene_pairs[num_pairs-1][1].getImageName()+"-"+
    						scene_pairs[0][0].getImageName()+"-"+num_pairs);
//    						dbg_titles);
    	}
        //combo_dsn_final
    	
    	// Re-calc BG?
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP] =        ref_disparity.clone();
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA] =         ref_disparity.clone();
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP_FG] =     ref_disparity.clone();
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP_BG_ALL] = ref_disparity.clone();
		String rslt_suffix = "-INTER-INTRA";
		rslt_suffix += (clt_parameters.correlate_lma?"-LMA":"-NOLMA");
		ref_scene.saveDoubleArrayInModelDirectory( // error
				rslt_suffix,           // String      suffix,
				OpticalFlow.COMBO_DSN_TITLES, // combo_dsn_titles_full, // null,          // String []   labels, // or null
				combo_dsn_final,       // dbg_data,         // double [][] data,
				tilesX,                // int         width,
				tilesY);               // int         height)
    	
        return combo_dsn_final;
	}

	
	public static double [][] sfmPair( // clean this up from extra data
			final CLTParameters          clt_parameters,
			final QuadCLT                ref_scene,
			final QuadCLT [][]           scene_pairs,
			final double                 mb_max_gain,
			final boolean                batch_mode,
			final int                    debugLevel) {
		boolean       show_disp_corr =       false; //true;
		boolean       show_disp_seq =        true;
		int           num_readjust =         5;
		final double  min_strength =         0.4; // update if correction strength exceeds
		final boolean use_neibs =            true;
		final double  min_neib_strength=     0.5; // update if no-individual and neibs correction strength exceeds 
		final double  prev_sfm_frac =        0.6; // update if new sfm gain > this fraction of the old one
		final double  range_disparity_offset = clt_parameters.imp.range_disparity_offset;
    	final double  centroid_radius = clt_parameters.imp.centroid_radius; // final double              centroid_radius, // 0 - use all tile, >0 - cosine window around local max
    	final int     n_recenter = clt_parameters.imp.n_recenter;           // when cosine window, re-center window this many times
		// 16 puts scale same as with older code
		final double   corr_fz_inter = 16* clt_parameters.getGpuFatZeroInter(ref_scene.isMonochrome());	
//		final int      corr_size = 2 * GPUTileProcessor.DTT_SIZE -1;
//		final double [] neib_weights_od = {0.7, 0.5};
		int tilesX =    ref_scene.getTileProcessor().getTilesX();
		int tilesY =    ref_scene.getTileProcessor().getTilesY();
		ErsCorrection ers_reference = ref_scene.getErsCorrection();		
		boolean use_combo_dsi =    true;
		boolean use_lma_dsi =      clt_parameters.imp.use_lma_dsi;
		double [][] ref_xyzatr =    new double [][] {ZERO3,ZERO3};
		double [][] ref_xyzatr_dt= {
				ers_reference.getErsXYZ_dt(),
				ers_reference.getErsATR_dt()
		};
		for (int i = 0; i < ref_xyzatr_dt.length; i++) {
			if (ref_xyzatr_dt[i] == null) {
				System.out.println("sfmPair(): ref_xyzatr_dt["+i+"] == null");
				ref_xyzatr_dt[i] = ZERO3.clone();
			}
		}

        double [] disparity_raw = new double [tilesX * tilesY];
        Arrays.fill(disparity_raw,clt_parameters.disparity);
        double [][] combo_dsn_final = ref_scene.restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky
        if (combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] == null) {
        	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] =
        			new double [combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP].length];
        	if (debugLevel > -3) {
        		System.out.println("sfmPair(): Initializing missing COMBO_DSN_INDX_SFM_GAIN data.");
        	}
        }
        final double [] ref_sfm_gain = combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN].clone();
        double [][] dls = { // Update to use FG? Or FG/no BG?
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP],
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA],
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_STRENGTH]
        };
        double [][] ds = OpticalFlow.conditionInitialDS(
        		true,                // boolean        use_conf,       // use configuration parameters, false - use following  
        		clt_parameters,      // CLTParameters  clt_parameters,
        		dls,                 // double [][]    dls
        		ref_scene,           // QuadCLT        scene,
        		debugLevel);			
        double [] interscene_ref_disparity = null; // keep null to use old single-scene disparity for interscene matching
        if (use_combo_dsi) {
        	interscene_ref_disparity = ds[0].clone(); // use_lma_dsi ?
        	if (use_lma_dsi) {
        		for (int i = 0; i < interscene_ref_disparity.length; i++) {
        			if (Double.isNaN(dls[1][i])) {
        				interscene_ref_disparity[i] = Double.NaN;
        			}
        		}
        	}
        }
        
        if (interscene_ref_disparity == null) {
        	interscene_ref_disparity =ref_scene.getDLS()[use_lma_dsi?1:0];
        }
        final double []   ref_disparity = interscene_ref_disparity;
        double [][] disp_adj = show_disp_seq? (new double [num_readjust+1][]):null;
        if (disp_adj != null) {
        	disp_adj[0] = ref_disparity.clone();
        }
    	final int num_pairs = scene_pairs.length;
    	final double[][][][] scenes_xyzatr = new double[num_pairs][2][][];   // 2 scenes
    	final double[][][][] scenes_xyzatr_dt  = new double[num_pairs][2][][];// 2 scenes
    	for (int npair = 0; npair < num_pairs; npair++) {
    		QuadCLT [] scenes = scene_pairs[npair];
    		for (int nscene = 0; nscene < scenes.length; nscene++) {
    			String ts = scenes[nscene].getImageName();
    			if (ts.equals(ref_scene.getImageName())) {
    				scenes_xyzatr[npair][nscene] = ref_xyzatr;
    				scenes_xyzatr_dt[npair][nscene]= ref_xyzatr_dt;
    			} else {
    				if ((ers_reference.getSceneXYZ(ts)== null) || (ers_reference.getSceneATR(ts)== null)) {
    					System.out.println("sfmPair(): no pose for timestamp "+ts);
    					return null;
    				}
    				scenes_xyzatr[npair][nscene] = new double[][] {
    					ers_reference.getSceneXYZ(ts),
    					ers_reference.getSceneATR(ts)};
    					scenes_xyzatr_dt[npair][nscene]= new double[][] {
    						ers_reference.getSceneErsXYZ_dt(ts),
    						ers_reference.getSceneErsATR_dt(ts)};
    						for (int i = 0; i < scenes_xyzatr_dt[npair][nscene].length; i++) {
    							if (scenes_xyzatr_dt[npair][nscene][i] == null) {
    								System.out.println("sfmPair(): scene_xyzatr_dt["+i+"] == null");
    								scenes_xyzatr_dt[npair][nscene][i] = ZERO3.clone();
    							}
    						}
    			}
    		}
    	}        
    	
        for (int ntry = 0; ntry < num_readjust; ntry++) {
        	SfmCorr [] sfmCorr = getSfmCorr(
        			clt_parameters,         // final CLTParameters  clt_parameters,
        			ref_scene,              // final QuadCLT        ref_scene,
        			ref_disparity,          // final double []      ref_disparity,
        			ref_xyzatr,             // final double[][]     ref_xyzatr,   // new double[num_pairs][2][][];   // 2 scenes
        			ref_xyzatr_dt,          // final double[][]     ref_xyzatr_dt, // new double[num_pairs][2][][];// 2 scenes
        			scene_pairs,            // final QuadCLT[][]    scene_pairs,
        			scenes_xyzatr,          // final double[][][][] scenes_xyzatr,   // new double[num_pairs][2][][];   // 2 scenes
        			scenes_xyzatr_dt,       // final double[][][][] scenes_xyzatr_dt, // new double[num_pairs][2][][];// 2 scenes
        			mb_max_gain,            // final double         mb_max_gain,
        			range_disparity_offset, // final double         range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ;			final double         mb_max_gain,
        			corr_fz_inter,          // final double         corr_fz_inter,
        			use_neibs,              // final boolean        use_neibs,
        			centroid_radius,        // final double       centroid_radius, // 0 - use all tile, >0 - cosine window around local max
        			n_recenter,             // final int            n_recenter,      // when cosine window, re-center window this many times			 
        			batch_mode,             // final boolean        batch_mode,
        			debugLevel);            // final int            debugLevel)
        	if (show_disp_corr) {
        		String [] dbg_titles = {"corr", "corr_str","neib_corr", "neib_str","old_sfm","new_sfm"};
        		
        		double [][] dbg_2d_corr = new double [dbg_titles.length][sfmCorr.length];
        		for (int i = 0; i < dbg_2d_corr.length; i++) {
        			Arrays.fill(dbg_2d_corr[i], Double.NaN);
        		}
        		dbg_2d_corr[4] = ref_sfm_gain.clone();
        		for (int nTile = 0; nTile < sfmCorr.length; nTile++) if (sfmCorr[nTile] != null) {
        			dbg_2d_corr[5][nTile] = sfmCorr[nTile].sfm_gain;
        			if (sfmCorr[nTile].corr_ind != null) {
        				dbg_2d_corr[0][nTile] = sfmCorr[nTile].corr_ind[0];
        				dbg_2d_corr[1][nTile] = sfmCorr[nTile].corr_ind[1];
        			}
        			if (sfmCorr[nTile].corr_neib != null) {
        				dbg_2d_corr[2][nTile] = sfmCorr[nTile].corr_neib[0];
        				dbg_2d_corr[3][nTile] = sfmCorr[nTile].corr_neib[1];
        			}
        		}
        		ShowDoubleFloatArrays.showArrays(
        				dbg_2d_corr,
        				tilesX,
        				tilesY,
        				true,
        				"sfm_corr-"+scene_pairs[num_pairs-1][1].getImageName()+"-"+
        						scene_pairs[0][0].getImageName()+"-"+num_pairs+"-"+String.format("%02d", ntry),
        						dbg_titles);
        	}
        	
// combine corrections
    		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.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 < sfmCorr.length; nTile = ai.getAndIncrement())
    						if (sfmCorr[nTile] != null){
    							if (sfmCorr[nTile].sfm_gain >= prev_sfm_frac*ref_sfm_gain[nTile]){
    								// Update some strength too?
    								if ((sfmCorr[nTile].corr_ind != null) && (sfmCorr[nTile].corr_ind[1] > min_strength)) {
    									ref_disparity[nTile] += sfmCorr[nTile].corr_ind[0];
    									ref_sfm_gain[nTile] = sfmCorr[nTile].sfm_gain; 
    								} else if ((sfmCorr[nTile].corr_neib != null) && (sfmCorr[nTile].corr_neib[1] > min_neib_strength)) {
    									ref_disparity[nTile] += sfmCorr[nTile].corr_neib[0];
    									ref_sfm_gain[nTile] = sfmCorr[nTile].sfm_gain; 
    								}
    							}
                        }
    				}
    			};
    		}		      
    		ImageDtt.startAndJoin(threads);
        	// replace weak tiles with average?
    		if (disp_adj != null) {
    			disp_adj[ntry + 1] = ref_disparity.clone();
    		}
        	
        } // ntry
    	if (disp_adj != null) {
    		ShowDoubleFloatArrays.showArrays(
    				disp_adj,
    				tilesX,
    				tilesY,
    				true,
    				"SfM-disparity-"+scene_pairs[num_pairs-1][1].getImageName()+"-"+
    						scene_pairs[0][0].getImageName()+"-"+num_pairs);
    	}
    	// Re-calc BG?
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP] =        ref_disparity.clone();
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA] =         ref_disparity.clone();
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP_FG] =     ref_disparity.clone();
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP_BG_ALL] = ref_disparity.clone();
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] =    ref_sfm_gain.clone(); 
    	
		String rslt_suffix = "-INTER-INTRA";
		rslt_suffix += (clt_parameters.correlate_lma?"-LMA":"-NOLMA");
		ref_scene.saveDoubleArrayInModelDirectory( // error
				rslt_suffix,           // String      suffix,
				OpticalFlow.COMBO_DSN_TITLES, // combo_dsn_titles_full, // null,          // String []   labels, // or null
				combo_dsn_final,       // dbg_data,         // double [][] data,
				tilesX,                // int         width,
				tilesY);               // int         height)
        return combo_dsn_final;
	}

	/**
	 * Calculate SfM disparity correction from a set of scene pairs with approximately
	 * the same position offset in each pair by consolidating 2D correlation data in
	 * Transform Domain before converting to the Pixel Domain. Optionally provide such
	 * correction for averaged with 8 immediate neighbors to deal with very low contrast
	 * areas. Additionally provide SfM gain (saved with the disparity data) to indicate
	 * estimated accuracy of disparity data.
	 * Reference scene disparity and scene poses are provided explicitly to allow tuning.
	 *   
	 * @param clt_parameters         processing parameters  
	 * @param ref_scene              reference scene instance
	 * @param ref_disparity          reference disparity array (NaN for undefined)
	 * @param ref_xyzatr             reference scene pose {{x,y,z},{a,t,r}}
	 * @param ref_xyzatr_dt          reference scene pose derivatives (for ERS)
	 * @param scene_pairs            pairs of scene instances to correlate ([1] to [0])
	 * @param scenes_xyzatr          pairs of scene poses relative to the reference one
	 * @param scenes_xyzatr_dt       pairs of scene poses derivatives (for ERS)
	 * @param mb_max_gain            motion blur correction maximal gain
	 * @param range_disparity_offset disparity at actual infinity. Here can be just 0.
	 * @param corr_fz_inter          Fat zero correction. Recommended value:
	 *                               (16* clt_parameters.getGpuFatZeroInter(ref_scene.isMonochrome())
	 * @param use_neibs              if true, will generate data for averaging between neighbors
	 * @param centroid_radius
	 * @param n_recenter
	 * @param batch_mode             batch mode (suppress all debug images) 
	 * @param debugLevel             debug level
	 * @return per tile array (sparse) of SfmCorr instances, containing tile's SfM gain,
	 *  individual disparity correction (to add to the current disparity)/strength and
	 *  optionally averaged between neighbors correction/strength 
	 */
	public static SfmCorr [] getSfmCorr(
			final CLTParameters  clt_parameters,
			final QuadCLT        ref_scene,
			final double []      ref_disparity,
        	final double[][]     ref_xyzatr,   // new double[num_pairs][2][][];   // 2 scenes
        	final double[][]     ref_xyzatr_dt, // new double[num_pairs][2][][];// 2 scenes
			final QuadCLT[][]    scene_pairs,
        	final double[][][][] scenes_xyzatr,   // new double[num_pairs][2][][];   // 2 scenes
        	final double[][][][] scenes_xyzatr_dt, // new double[num_pairs][2][][];// 2 scenes
        	final double         mb_max_gain,
        	final double         range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ;			final double         mb_max_gain,
			final double         corr_fz_inter,
			final boolean        use_neibs,
			final double         centroid_radius, // 0 - use all tile, >0 - cosine window around local max
			final int            n_recenter,      // when cosine window, re-center window this many times			 
			final boolean        batch_mode,
			final int            debugLevel) {
		
		final int  num_pairs = scene_pairs.length;
		double []  neib_weights_od = {0.7, 0.5};
		final int  corr_size = 2 * ref_scene.tp.getTileSize() -1;
		int tilesX = ref_scene.tp.getTilesX();
    	// only for derivatives
    	double [][] ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
    			null, // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
    			ref_disparity, // dls[0],  // final double []   disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
    			ZERO3,         // final double []   scene_xyz, // camera center in world coordinates
    			ZERO3,         // final double []   scene_atr, // camera orientation relative to world frame
    			ref_scene,     // final QuadCLT     scene_QuadClt,
    			ref_scene);    // final QuadCLT     reference_QuadClt)
    	double [][][] dpXYddisp = new double [num_pairs][][]; // will be averaged between all pairs
    	TDCorrTile [] accum_TD = null;
    	for (int npair = 0; npair < num_pairs; npair++) {
    		dpXYddisp[npair] = getSfmDpxDpyDdisp( // will be averaged
        			scenes_xyzatr[0],          // final double [][][] scenes_xyzatr,
        			scene_pairs[0][0], // final QuadCLT       scene0,
        			scene_pairs[0][1], // final QuadCLT       scene1,
        			ref_scene,              // final QuadCLT       ref_QuadClt,
        			ref_pXpYD,              // final double [][]   ref_pXpYD, // centers
        			range_disparity_offset, // final double        range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ;
        			batch_mode,             // final boolean       batch_mode,
        			debugLevel);            // final int           debug_level
    		QuadCLT [] scenes = scene_pairs[npair];
    		TDCorrTile [] pair_TD =sfmPair_TD( // scene0/scene1
    				clt_parameters,  // final CLTParameters          clt_parameters,
    				ref_scene,       // final QuadCLT                ref_scene,
    				ref_disparity,   // final double []              ref_disparity, // here can not be null
    				ref_xyzatr,      // final double [][]            ref_xyzatr, // {ZERO3,ZERO3};
    				ref_xyzatr_dt,   // final double [][]            ref_xyzatr_dt,
    				scenes,          // final QuadCLT []             scenes,          // 2 scenes
    				scenes_xyzatr[npair],   //final double[][][]	         scenes_xyzatr,   // 2 scenes
    				scenes_xyzatr_dt[npair],// final double[][][]	         scenes_xyzatr_dt,// 2 scenes
    				mb_max_gain,     // double                       mb_max_gain,
    				batch_mode,      // final boolean                batch_mode,
    				debugLevel);     // final int                    debugLevel);
    		if (npair == 0 ) {
    			accum_TD = pair_TD;
    		} else {
    			TDCorrTile.accumulate (accum_TD, pair_TD);
    		}
    	}
    	double [][] accum_PD = TDCorrTile.convertTDtoPD(
				ref_scene.getGPU(), // final GpuQuad       gpuQuad,
				accum_TD,           // final TDCorrTile [] tiles,
				0xFE,               // final int           corr_type, // 0xFE
				corr_fz_inter,      // final double        gpu_fat_zero,
				debugLevel);        // final int           debug_level
    	double [][] neibs_PD = null;
    	if (use_neibs) {
    		TDCorrTile [] neibs_TD = TDCorrTile.calcNeibs(
    				accum_TD, // pairs_TD,        // TDCorrTile [] tiles,
    				tilesX,              // final int     tilesX,
    				neib_weights_od,     //double []     neib_weights_od, // {orhto, diag}
    				true);               // final boolean process_all
    		neibs_PD = TDCorrTile.convertTDtoPD(
    				ref_scene.getGPU(), // final GpuQuad       gpuQuad,
    				neibs_TD,           // final TDCorrTile [] tiles,
    				0xFE,               // final int           corr_type, // 0xFE
    				corr_fz_inter,      // final double        gpu_fat_zero,
    				debugLevel);        // final int           debug_level
    	}
    	final double [][] dpXYddisp_avg = new double [accum_PD.length][];
		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.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 < accum_PD.length; nTile = ai.getAndIncrement()) {
						double [] sxy = new double[2];
						int na = 0;
						for (int npair = 0; npair < num_pairs; npair++) {
							if (dpXYddisp[npair][nTile] != null) {
								for (int i = 0; i < 2; i++) {
									sxy[i]+=dpXYddisp[npair][nTile][i];
								}
								na++;
							}
						}
						if (na > 0) {
							for (int i = 0; i < 2; i++) {
								sxy[i] /= na;
							}
							dpXYddisp_avg[nTile] = sxy;
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		
    	final SfmCorr [] sfmCorr = new SfmCorr [accum_PD.length];
    	final double [][][] data_PD = (neibs_PD == null) ? (new double[][][] {accum_PD}):(new double[][][] {accum_PD,neibs_PD});
    	final double [][][] disp_corr = new double [data_PD.length][][];
    	for (int i = 0; i < data_PD.length; i++) {
    		disp_corr[i] = getSfmDisparityCorrectionStrength(
    				clt_parameters,  // final CLTParameters  clt_parameters,
    				data_PD[i],      // final double [][]    corr2d_PD,
    				dpXYddisp_avg,   // final double [][]    dpXYddisp,
    				centroid_radius, // final double         centroid_radius, // 0 - use all tile, >0 - cosine window around local max
    				n_recenter,      // final int            n_recenter,      // when cosine window, re-center window this many times
    				corr_size,       //final int            corr_size, // 15
    				debugLevel);     // final int            debugLevel);
    	}
    	
    	ai.set(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile < accum_PD.length; nTile = ai.getAndIncrement()) {
//						if (nTile==4600) {
//							System.out.println("getSfmCorr(): nTile="+nTile);
//						}
						if ((disp_corr[0][nTile] != null) || (use_neibs && (disp_corr[1][nTile] != null) )) {
							sfmCorr[nTile] = new SfmCorr();
							double [] sxy = dpXYddisp_avg[nTile];
							sfmCorr[nTile].sfm_gain = Math.sqrt(sxy[0]*sxy[0] + sxy[1]*sxy[1]);
							sfmCorr[nTile].corr_ind = disp_corr[0][nTile];
							if (use_neibs) {
								sfmCorr[nTile].corr_neib = disp_corr[1][nTile];
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
    	return sfmCorr;
	}
	
	
	/**
	 * Calculate disparity correction from the pixel-domain 2D correlation data
	 * and the SfM gain vector array (nominal disparity pixels per SfM correlation
	 * pixels in X and Y directions.
	 * @param clt_parameters   processing parameters 
	 * @param corr2d_PD        per-tile sparse array of the 2D correlation results 
	 * @param dpXYddisp        per-tile sparse array of the X,Y components of teh SfM gain
	 * @param centroid_radius  windowing for centroid argmax: 0 - use all tile, >0 - cosine
	 *                         window around local max 
	 * @param n_recenter       number of refining of the argmax
	 * @param corr_size        2d correlation tile size (side of a square, normally 15) 
	 * @param debugLevel       debug level
	 * @return   per tile sparse array of {disparity_correction, strength} pairs              
	 */
	public static double [][] getSfmDisparityCorrectionStrength(
			final CLTParameters  clt_parameters,
			final double [][]    corr2d_PD,
			final double [][]    dpXYddisp,
			final double         centroid_radius, // 0 - use all tile, >0 - cosine window around local max
			final int            n_recenter,      // when cosine window, re-center window this many times
			final int            corr_size, // 15
			final int            debugLevel) {
		final double [][] disp_corr = new double [corr2d_PD.length][];
		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.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 < corr2d_PD.length; nTile = ai.getAndIncrement())
						if ((corr2d_PD[nTile] != null) && (dpXYddisp[nTile] != null)){
							double [] v =     dpXYddisp[nTile];
							double l2 =       v[0]*v[0]+ v[1]*v[1];
							double [] inv_v = {v[0]/l2, v[1]/l2}; 
							double [] corr_cm = Correlation2d.getMaxProjCm(
									corr2d_PD[nTile], // double []  data,
									corr_size,        // int        data_width,   //  = 2 * transform_size - 1;
									centroid_radius,  // double     radius,       //  0 - all same weight, > 0 cosine(PI/2*sqrt(dx^2+dy^2)/rad)
									n_recenter,       // int        refine,       //  re-center window around new maximum. 0 -no refines (single-pass)
									inv_v,            // double []  direction_XY,
									debugLevel > 2);  // boolean    debug)
							disp_corr[nTile] = corr_cm; // may be null
						}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return disp_corr;
	}
	
	
	/**
	 * Calculate a transform-domain cross-correlation of 2 scenes, keep only all-channels combined
	 * as a TDCorrTile [] sparse array. Each non-null element contains weight (normally 16 as number of
	 * channels combined) and 256 elements of the TD representation of the correlation data.
	 * Reference scene disparity and scene poses are provided explicitly to allow tuning.
	 * Both scenes are separate from the reference scene (TODO: verify when 1 is the reference) 
	 * 
	 * @param clt_parameters   processing parameters  
	 * @param ref_scene        reference scene instance
	 * @param ref_disparity    reference disparity array (NaN for undefined)
	 * @param ref_xyzatr       reference scene [2][3] pose {{x,y,z},{a,t,r}}
	 * @param ref_xyzatr_dt    reference scene pose derivatives (for ERS)
	 * @param scenes           pair of scene instances to correlate ([1] to [0])
	 * @param scenes_xyzatr    pair of scene poses relative to the reference one
	 * @param scenes_xyzatr_dt pair of scene poses derivatives (for ERS)
	 * @param mb_max_gain      motion blur correction maximal gain
	 * @param batch_mode       batch mode (suppress all debug images) 
	 * @param debugLevel       debug level
	 * @return TDCorrTile [] sparse array representing a transform-domain interscene correlation
	 *         result.
	 */
	public static TDCorrTile [] sfmPair_TD( // scene0/scene1
			final CLTParameters          clt_parameters,
			final QuadCLT                ref_scene,
			final double []              ref_disparity, // here can not be null
			final double [][]            ref_xyzatr, // {ZERO3,ZERO3};
			final double [][]            ref_xyzatr_dt,
			final QuadCLT []             scenes,          // 2 scenes
			final double[][][]	         scenes_xyzatr,   // 2 scenes
			final double[][][]	         scenes_xyzatr_dt,// 2 scenes
			double                       mb_max_gain,
			final boolean                batch_mode,
			final int                    debugLevel) {
		int                imp_debug_level = 0; // MANUALLY change to 2 for debug!
		boolean show_render_ref =      !batch_mode && clt_parameters.imp.renderRef(imp_debug_level); // false; //true;
		boolean show_render_scene =    !batch_mode && clt_parameters.imp.renderScene(imp_debug_level); // false; // true;
		boolean toRGB = true;		
		boolean mb_en =       clt_parameters.imp.mb_en;
		double  mb_tau =      clt_parameters.imp.mb_tau;      // 0.008; // time constant, sec
		int     margin =      clt_parameters.imp.margin;
		int        sensor_mask_inter = clt_parameters.imp.sensor_mask_inter ; //-1;
        double [][][] scenes_pXpYD = new double [scenes.length][][]; 
        // ref_pXpYD should correspond to uniform grid (do not undo ERS of the reference scene)
		double [][] ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
        		null, // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
        		ref_disparity, // dls[0],  // final double []   disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
        		ZERO3,         // final double []   scene_xyz, // camera center in world coordinates
        		ZERO3,         // final double []   scene_atr, // camera orientation relative to world frame
        		ref_scene,     // final QuadCLT     scene_QuadClt,
        		ref_scene);    // final QuadCLT     reference_QuadClt)
        
		@SuppressWarnings("unused")
		TpTask[][] tp_tasks_ref; // only were used for the FPN removal
		double [][][]  mb_vectors_scenes = new double [scenes.length][][];
		
		for (int nscene = 0; nscene < scenes.length; nscene++) {
			scenes_pXpYD[nscene] = OpticalFlow.transformToScenePxPyD( // will be null for disparity == NaN, total size - tilesX*tilesY
					null,                     // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
					ref_disparity,            // dls[0], // final double []   disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
					scenes_xyzatr[nscene][0], // final double []   scene_xyz, // camera center in world coordinates
					scenes_xyzatr[nscene][1], // final double []   scene_atr, // camera orientation relative to world frame
					scenes[nscene],           // final QuadCLT     scene_QuadClt,
					ref_scene);               // final QuadCLT     reference_QuadClt)
					
			if (mb_en) {
				mb_vectors_scenes[nscene] = OpticalFlow.getMotionBlur(
						ref_scene,                  // QuadCLT        ref_scene,
						scenes[nscene],             // QuadCLT        scene,         // can be the same as ref_scene
						ref_pXpYD,                  // double [][]    ref_pXpYD,     // here it is scene, not reference!
						scenes_xyzatr[nscene][0],   // double []      camera_xyz,
						scenes_xyzatr[nscene][1],   // double []      camera_atr,
						scenes_xyzatr_dt[nscene][0],// double []      camera_xyz_dt,
						scenes_xyzatr_dt[nscene][1],// double []      camera_atr_dt,
						0,                          // int            shrink_gaps,  // will gaps, but not more that grow by this
						debugLevel);                // int            debug_level)
			}
		}
		// remove tiles from both that are null for any
		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.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 < ref_pXpYD.length; nTile = ai.getAndIncrement())
						if (ref_pXpYD[nTile] != null){
							boolean undefined = false;
							for (int nscene = 0; nscene < scenes.length; nscene++) {
								if (scenes_pXpYD[nscene][nTile] == null) {
									undefined=true;
									break;
								}
							}
							if (undefined) {
								for (int nscene = 0; nscene < scenes.length; nscene++) {
									scenes_pXpYD[nscene][nTile] = null;
								}								
							}
						
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);

		
		tp_tasks_ref = // were used only for the FPN removal
				Interscene.setReferenceGPU (
				clt_parameters,      // CLTParameters      clt_parameters,			
				scenes[0],           // ref_scene,                 // QuadCLT            ref_scene,
				null,                // ref_disparity,             // double []          ref_disparity, // null or alternative reference disparity
				scenes_pXpYD[0],     // ref_pXpYD,                 // double [][]        ref_pXpYD,
				null,                // reliable_ref,              // final boolean []   selection, // may be null, if not null do not  process unselected tiles
				margin,              // final int          margin,
				// motion blur compensation 
				mb_tau,              // double             mb_tau,      // 0.008; // time constant, sec
				mb_max_gain,         // double             mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
				mb_vectors_scenes[0],// mb_vectors_ref,            // double [][]        mb_vectors,  // now [2][ntiles];
				debugLevel);         // int         debugLevel)
		// is it needed here?
		scenes[0].saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry

		ImageDtt image_dtt;
		image_dtt = new ImageDtt(
				ref_scene.getNumSensors(), // ,
				clt_parameters.transform_size,
				clt_parameters.img_dtt,
				ref_scene.isAux(),
				ref_scene.isMonochrome(),
				ref_scene.isLwir(),
				clt_parameters.getScaleStrength(ref_scene.isAux()),
				ref_scene.getGPU());
		image_dtt.getCorrelation2d(); // initiate image_dtt.correlation2d, needed if disparity_map != null
		//ref_scene.getGPU() can not be null
		ref_scene.getGPU().setGpu_debug_level(debugLevel - 4); // monitor GPU ops >=-1
		final double disparity_corr = clt_parameters.imp.disparity_corr; // 04/07/2023 // 0.0; // (z_correction == 0) ? 0.0 : geometryCorrection.getDisparityFromZ(1.0/z_correction);
		final double gpu_sigma_corr =     clt_parameters.getGpuCorrSigma(ref_scene.isMonochrome());
		final double gpu_sigma_rb_corr =  ref_scene.isMonochrome()? 1.0 : clt_parameters.gpu_sigma_rb_corr;
		final double gpu_sigma_log_corr = clt_parameters.getGpuCorrLoGSigma(ref_scene.isMonochrome());
		
		TpTask[][] tp_tasks;
		if (mb_en && (mb_vectors_scenes[1]!=null)) {
			tp_tasks  =  GpuQuad.setInterTasksMotionBlur(
					scenes[1].getNumSensors(),
					scenes[1].getErsCorrection().getSensorWH()[0],
					!scenes[1].hasGPU(),          // final boolean             calcPortsCoordinatesAndDerivatives, // GPU can calculate them centreXY
					scenes_pXpYD[1],              // final double [][]         pXpYD, // per-tile array of pX,pY,disparity triplets (or nulls)
					null,                         // final boolean []          selection, // may be null, if not null do not  process unselected tiles
					// motion blur compensation 
					mb_tau,                       // final double              mb_tau,      // 0.008; // time constant, sec
					mb_max_gain,                  // final double              mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
					mb_vectors_scenes[1],         // final double [][]         mb_vectors,  //
					scenes[1].getErsCorrection(), // final GeometryCorrection  geometryCorrection,
					disparity_corr,               // final double              disparity_corr,
					margin,                       // final int                 margin,      // do not use tiles if their centers are closer to the edges
					null,                         // final boolean []          valid_tiles,            
					THREADS_MAX);                 // final int                 threadsMax)  // maximal number of threads to launch
		} else {
			tp_tasks = new TpTask[1][];
			tp_tasks[0]  =  GpuQuad.setInterTasks(
					scenes[1].getNumSensors(),
					scenes[1].getErsCorrection().getSensorWH()[0],
					!scenes[1].hasGPU(),          // final boolean             calcPortsCoordinatesAndDerivatives, // GPU can calculate them centreXY
					scenes_pXpYD[1],              // final double [][]         pXpYD, // per-tile array of pX,pY,disparity triplets (or nulls)
					null, // selection,           // final boolean []          selection, // may be null, if not null do not  process unselected tiles
					scenes[1].getErsCorrection(), // final GeometryCorrection  geometryCorrection,
					disparity_corr,               // final double              disparity_corr,
					margin,                       // final int                 margin,      // do not use tiles if their centers are closer to the edges
					null,                         // final boolean []          valid_tiles,            
					THREADS_MAX);                 // final int                 threadsMax)  // maximal number of threads to launch
		}
		scenes[1].saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
		// Generate 2D phase correlations from the CLT representation
		// generates sum of the per-channel correlations as the last slot.
		// updates gpuQuad.gpu_corr_indices, gpuQuad.gpu_corrs_td and some other
		if (mb_en && (mb_vectors_scenes[1]!=null)) {
			image_dtt.interCorrTDMotionBlur(
					clt_parameters.img_dtt,     // final ImageDttParameters  imgdtt_params,    // Now just extra correlation parameters, later will include, most others
					tp_tasks,                   // final TpTask[][]            tp_tasks,
					null,                       // final float  [][][][]     fcorr_td,        // [tilesY][tilesX][pair][4*64] transform domain representation of 6 corr pairs
					clt_parameters.gpu_sigma_r, // final double              gpu_sigma_r,     // 0.9, 1.1
					clt_parameters.gpu_sigma_b, // final double              gpu_sigma_b,     // 0.9, 1.1
					clt_parameters.gpu_sigma_g, // final double              gpu_sigma_g,     // 0.6, 0.7
					clt_parameters.gpu_sigma_m, // final double              gpu_sigma_m,     //  =       0.4; // 0.7;
					gpu_sigma_rb_corr,          // final double              gpu_sigma_rb_corr,    //  = 0.5; // apply LPF after accumulating R and B correlation before G, monochrome ? 1.0 :
					gpu_sigma_corr,             // final double              gpu_sigma_corr,       //  =    0.9;gpu_sigma_corr_m
					gpu_sigma_log_corr,         // final double              gpu_sigma_log_corr,   // hpf to reduce dynamic range for correlations
					clt_parameters.corr_red,    // final double              corr_red, // +used
					clt_parameters.corr_blue,   // final double              corr_blue,// +used
					sensor_mask_inter,          // final int                 sensor_mask_inter, // The bitmask - which sensors to correlate, -1 - all.
					THREADS_MAX,                // final int                 threadsMax,       // maximal number of threads to launch
					debugLevel);                // final int                 globalDebugLevel);
		} else {
			image_dtt.interCorrTD(
					clt_parameters.img_dtt,     // final ImageDttParameters  imgdtt_params,    // Now just extra correlation parameters, later will include, most others
					tp_tasks[0],                // final TpTask[]            tp_tasks,
					null,                       // final float  [][][][]     fcorr_td,        // [tilesY][tilesX][pair][4*64] transform domain representation of 6 corr pairs
					clt_parameters.gpu_sigma_r, // final double              gpu_sigma_r,     // 0.9, 1.1
					clt_parameters.gpu_sigma_b, // final double              gpu_sigma_b,     // 0.9, 1.1
					clt_parameters.gpu_sigma_g, // final double              gpu_sigma_g,     // 0.6, 0.7
					clt_parameters.gpu_sigma_m, // final double              gpu_sigma_m,     //  =       0.4; // 0.7;
					gpu_sigma_rb_corr,          // final double              gpu_sigma_rb_corr,    //  = 0.5; // apply LPF after accumulating R and B correlation before G, monochrome ? 1.0 :
					gpu_sigma_corr,             // final double              gpu_sigma_corr,       //  =    0.9;gpu_sigma_corr_m
					gpu_sigma_log_corr,         // final double              gpu_sigma_log_corr,   // hpf to reduce dynamic range for correlations
					clt_parameters.corr_red,    // final double              corr_red, // +used
					clt_parameters.corr_blue,   // final double              corr_blue,// +used
					sensor_mask_inter,          // final int                 sensor_mask_inter, // The bitmask - which sensors to correlate, -1 - all.
					THREADS_MAX,                // final int                 threadsMax,       // maximal number of threads to launch
					debugLevel);                // final int                 globalDebugLevel);
		}
		// get TD interscene correlation of 2 scenes, use only combo (all channels) data
		TDCorrTile [] tiles = TDCorrTile.getFromGpu(
				ref_scene.getGPU());

		if (show_render_ref) {
			ImagePlus imp_render_ref = scenes[0].renderFromTD (
					-1,                  // final int         sensor_mask,
					false,               // boolean             merge_channels,
					clt_parameters,                                 // CLTParameters clt_parameters,
					clt_parameters.getColorProcParameters(ref_scene.isAux()), //ColorProcParameters colorProcParameters,
					clt_parameters.getRGBParameters(),              //EyesisCorrectionParameters.RGBParameters rgbParameters,
					null, // int []  wh,
					toRGB, // boolean toRGB,
					true, //boolean use_reference
					"GPU-SHIFTED-REF"); // String  suffix)
			imp_render_ref.show();
		}
		if (show_render_scene) { //set toRGB=false
			ImagePlus imp_render_scene = scenes[1].renderFromTD (
					-1,                  // final int         sensor_mask,
					false,               // boolean             merge_channels,
					clt_parameters,                                 // CLTParameters clt_parameters,
					clt_parameters.getColorProcParameters(ref_scene.isAux()), //ColorProcParameters colorProcParameters,
					clt_parameters.getRGBParameters(),              //EyesisCorrectionParameters.RGBParameters rgbParameters,
					null, // int []  wh,
					toRGB, // boolean toRGB,
					false, //boolean use_reference
					"GPU-SHIFTED-SCENE"); // String  suffix)
			imp_render_scene.show();
		}
		return tiles;
	}
		
	public static double [][]   getSfmDpxDpyDdisp(
			final double [][][] scenes_xyzatr, // cameras center in world coordinates (or null to use instance)
			final QuadCLT       scene0,
			final QuadCLT       scene1,
			final QuadCLT       ref_QuadClt,
			final double [][]   ref_pXpYD, // centers
			final double        range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ;
			final boolean       batch_mode,
			final int           debug_level
			){
		boolean debug_img = !batch_mode && (debug_level > 0);
		double [][] dbg_img = debug_img? (new double[6] [ref_pXpYD.length]):null;
		if (dbg_img != null) {
			for (int i = 0; i <dbg_img.length;i++) {
				Arrays.fill(dbg_img[i],Double.NaN);
			}
		}
		final QuadCLT[]     scenes = {scene0, scene1};
		boolean[] param_select = new boolean[ErsCorrection.DP_NUM_PARS];
		final int [] par_indices = new int[] {
				ErsCorrection.DP_DD};
		for (int i: par_indices) {
			param_select[i]=true;
		}
		final double [][][] last_jts = new double [scenes.length][][];
		IntersceneLma intersceneLma= new IntersceneLma(
				false,            // clt_parameters.ilp.ilma_thread_invariant);
				0.0);             // always no disparity
		for (int nscene = 0; nscene < scenes.length; nscene++) {
			// try if one is reference?
			if (scenes[nscene].getImageName().equals(ref_QuadClt.getImageName())) {
				last_jts[nscene] = new double[1][2 * ref_pXpYD.length];
			} else {
				intersceneLma.prepareLMA(
						scenes_xyzatr[nscene][0], // final double []   scene_xyz0,     // camera center in world coordinates (or null to use instance)
						scenes_xyzatr[nscene][1], // final double []   scene_atr0,     // camera orientation relative to world frame (or null to use instance)
						null,             // final double []   scene_xyz_pull, // if both are not null, specify target values to pull to 
						null,             // final double []   scene_atr_pull, // 
						scenes[nscene],   // final QuadCLT     scene_QuadClt,
						ref_QuadClt,      // final QuadCLT     reference_QuadClt,
						param_select,     // final boolean[]   param_select,
						null,             // final double []   param_regweights,
						null,             // final double [][] vector_XYS, // optical flow X,Y, confidence obtained from the correlate2DIterate()
						ref_pXpYD,        // final double [][] centers,    // macrotile centers (in pixels and average disparities
						false,            // boolean           first_run,
						debug_level);     // final int         debug_level)
				last_jts[nscene] = intersceneLma. getLastJT(); // alternating x,y for each selected parameters
			}
		}
		
		int [] sensor_wh =    ref_QuadClt.getGeometryCorrection().getSensorWH();
		final double width =  sensor_wh[0];
		final double height = sensor_wh[1];
		final double min_disparity = .05; // -0.5;
		final double max_disparity = 100.0;
		final double [][] dpXYddisp = new double [ref_pXpYD.length][];
		
		final int debug_tile = -1;
		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.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 < ref_pXpYD.length; nTile = ai.getAndIncrement()) if (ref_pXpYD[nTile] != null){
						if (nTile == debug_tile) {
							System.out.println("getSfmDpxDpyDdisp(): nTile="+nTile);
						}
						double disp_eff = ref_pXpYD[nTile][2] - range_disparity_offset;
						if (    (ref_pXpYD[nTile][0] < 0) || (ref_pXpYD[nTile][0] >= width) ||
								(ref_pXpYD[nTile][1] < 0) || (ref_pXpYD[nTile][1] >= height) ||
								(disp_eff < min_disparity) || (disp_eff >= max_disparity)) {
							continue;
						}
						dpXYddisp[nTile] = new double [2];
						dpXYddisp[nTile][0] = last_jts[1][0][2*nTile + 0]-last_jts[0][0][2*nTile + 0];
						dpXYddisp[nTile][1] = last_jts[1][0][2*nTile + 1]-last_jts[0][0][2*nTile + 1];
				        if (dbg_img != null) {
				            dbg_img[0][nTile] = dpXYddisp[nTile][0]; 
				            dbg_img[1][nTile] = dpXYddisp[nTile][1];
				            dbg_img[2][nTile] = last_jts[0][0][2*nTile + 0]; 
				            dbg_img[3][nTile] = last_jts[0][0][2*nTile + 1]; 
				            dbg_img[4][nTile] = last_jts[1][0][2*nTile + 0]; 
				            dbg_img[5][nTile] = last_jts[1][0][2*nTile + 1]; 
				        }
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		if (dbg_img != null) {
			int tilesX = ref_QuadClt.tp.getTilesX();
			int tilesY = ref_QuadClt.tp.getTilesY();
			String [] dbg_titles = {"dPx_dd","dPy_dd","dPx0_dd","dPy0_dd","dPx1_dd","dPy1_dd"}; // ,"dPx_dZ0","dPy_dZ0","dPx_dZ","dPy_dZ"};
        	ShowDoubleFloatArrays.showArrays(
        			dbg_img,
        			tilesX,
        			tilesY,
        			true,
        			"getSfmDpxDpyDdisp-"+ref_QuadClt.getImageName(),
        			dbg_titles);
		}		
		return dpXYddisp;
	}
}
