package com.elphel.imagej.cuas;

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.GpuQuad;
import com.elphel.imagej.gpu.TpTask;
import com.elphel.imagej.ims.UasLogReader;
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.TwoQuadCLT;

import ij.ImagePlus;
import ij.ImageStack;

public class CuasRanging {
	final QuadCLT           center_CLT;
	final QuadCLT []        scenes;
	final CLTParameters     clt_parameters;
	CuasMotion              cuasMotion = null;
	public int debugLevel = 0;
	
	public CuasRanging 	(
			CLTParameters     clt_parameters,
			QuadCLT           center_CLT,
			QuadCLT []        scenes,
			int               debugLevel) {
		this.clt_parameters = clt_parameters;
		this.center_CLT = center_CLT;
		this.scenes = scenes;
	}
	
	public CuasMotion detectTargets(
			UasLogReader    uasLogReader,
			boolean         batch_mode){
		boolean save_linear_cuas = true;
		boolean save_um_cuas = true;
//		boolean um_mono =            clt_parameters.imp.um_mono;
		double  um_sigma =           clt_parameters.imp.um_sigma;
		double  um_weight =          clt_parameters.imp.um_weight;
		boolean mono_fixed =         clt_parameters.imp.mono_fixed;
		double  mono_range =         clt_parameters.imp.mono_range;
//		boolean rng_um =             clt_parameters.imp.cuas_rng_um;
//		boolean rng_um_all =         clt_parameters.imp.cuas_rng_um_all;
		double rng_um_sigma =        clt_parameters.imp.cuas_rng_um_sigma;
//		double rng_um_weight =       clt_parameters.imp.cuas_rng_um_weight;
		double rng_radius =          clt_parameters.imp.cuas_rng_radius;
		double rng_blur =            clt_parameters.imp.cuas_rng_blur;
		
		boolean rng_img =            clt_parameters.imp.cuas_rng_img;
		boolean rng_disp =           clt_parameters.imp.cuas_rng_disp;
		
		double [][]combo_dsi = center_CLT.comboFromMain();
    	double [][] dls = { 
    			combo_dsi[OpticalFlow.COMBO_DSN_INDX_DISP], // **** null on second scene sequence
    			combo_dsi[OpticalFlow.COMBO_DSN_INDX_LMA],
    			combo_dsi[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
    			center_CLT,          // quadCLTs[ref_index], // QuadCLT        scene,
    			debugLevel-1);			
    	double [] disparity_fg = ds[0]; // combo_dsn_final[COMBO_DSN_INDX_DISP_FG];
    	double [] strength_fg =  ds[1];
		if (strength_fg != null) { // for FG/BG only, fixing for transformCameraVew()
			for (int i = 0; i < disparity_fg.length; i++) {
				if (!Double.isNaN(disparity_fg[i]) && (strength_fg[i] == 0)) strength_fg[i] = 0.01; // transformCameraVew ignores strength= 0
			}
		}
    	
    	double [] xyz_offset= {0,0,0};
		double [][] ds_vantage = new double[][] {disparity_fg, strength_fg};

		boolean debug_vantage = false;
    	double [][] dbg_vantage = debug_vantage ? (new double[7][]): null;
		if (dbg_vantage != null) {
			for (int i = 0; i < 3; i++) {
				dbg_vantage[i] = dls[i].clone();
			}
			for (int i = 0; i < 2; i++) {
				dbg_vantage[i+3] = ds_vantage[i].clone();
			}
		}
		ds_vantage = OpticalFlow.transformCameraVew(
				null,              // (debug_ds_fg_virt?"transformCameraVew":null),     // final String    title,
				ds_vantage,        // final double [][] dsrbg_camera_in,
				xyz_offset,        // xyz_offset, // _inverse[0], // final double [] scene_xyz, // camera center in world coordinates
				OpticalFlow.ZERO3, // _inverse[1], // final double [] scene_atr, // camera orientation relative to world frame
				center_CLT,        // quadCLTs[ref_index],      // final QuadCLT   scene_QuadClt,
				center_CLT,        // quadCLTs[ref_index],      // final QuadCLT   reference_QuadClt,
				8); // iscale);                  // final int       iscale);
		if (dbg_vantage != null) {
			for (int i = 0; i < 2; i++) {
				dbg_vantage[i+5] = ds_vantage[i].clone();
			}
    		ShowDoubleFloatArrays.showArrays(
    				dbg_vantage,
    				center_CLT.getTileProcessor().getTilesX(),
    				center_CLT.getTileProcessor().getTilesY(),
    				true,
    				center_CLT.getImageName()+"-ds_vantage", // "-corr2d"+"-"+frame0+"-"+frame1+"-"+corr_pairs,
    				new String[] {"disp0","lma0", "str0", "disp","str","virt_disp", "virt_str"});
		}

//		float [] average_pixels = (center_CLT.getCenterAverage() != null) ? ((float []) center_CLT.getCenterAverage().getProcessor().getPixels()):null;
		float [] average_pixels =    (float []) center_CLT.getCenterAverage().getProcessor().getPixels();
		float [][] average_channels = new float [][] {average_pixels}; // for future color images

		double [] cuas_atr = OpticalFlow.ZERO3;
		/*
		boolean extract_center_orientation = false; //  clt_parameters.imp.extract_center_orientation; // true; // false; // true; 
		double [][] center_ATR = null; //  {{center_A, center_T, average_R},{radius_A, radius_T}}
		if (extract_center_orientation && clt_parameters.imp.lock_position) {
			// cuas_atr will be use for rendering combo images, the individual coordinate will be already image-based, not ims-based
			center_ATR = CuasCenterLma.getCenterATR(
					scenes,                                 // QuadCLT [] quadCLTs,
					scenes[ref_index],                      // QuadCLT    ref_scene,ref_index,   //int        ref_index,
					new int [] {earliest_scene, last_index},  // int     []     range,
    				true, // 			boolean    disable_AT_omegas,
					debugLevel); // int debugLevel);
			cuas_atr = new double [] { center_ATR[0][0], center_ATR[0][1], center_ATR[0][2]};
			// Check omegas here !
			System.out.println ("Omegas ATR: "+center_ATR[2][0]+", "+center_ATR[2][1]+", "+center_ATR[2][2]);
		}
		*/
		String scenes_suffix = center_CLT.getImageName()+"-CUAS"; // "1747829900_781803-SEQ-FG-MONO-FPN";
		ImagePlus imp_targets= OpticalFlow.renderSceneSequence(
				clt_parameters,     // CLTParameters clt_parameters,
				true,  // center_CLT.hasCenterClt(), // boolean        mode_cuas,
				false, // clt_parameters.imp.um_mono, // boolean        um_mono,
				clt_parameters.imp.calculate_average,      // boolean        insert_average, // then add new parameter, keep add average
				average_channels,     //  average_slice,
				clt_parameters.imp.subtract_average, // boolean    subtract_average,
				clt_parameters.imp.running_average,  // int            running_average,
				null, // fov_tiles,   // Rectangle     fov_tiles,
				1, // mode3d,         // int           mode3d,
				false, // toRGB,      // boolean       toRGB,
				xyz_offset,           // double []     stereo_offset, // offset reference camera {x,y,z}
				cuas_atr,             // double []      stereo_atr, // offset reference orientation (cuas)
				1, // sensor_mask,    // int           sensor_mask,
				scenes_suffix,        // String        suffix,
				ds_vantage[0],        // selected_disparity, // double []     ref_disparity,			
				scenes,               // QuadCLT []    quadCLTs,
				center_CLT,           // ref_index,          // int           ref_index,
				ImageDtt.THREADS_MAX, // threadsMax,         // int           threadsMax,
				debugLevel);          // int           debugLevel);
        if (save_linear_cuas) {	   
        	center_CLT.saveImagePlusInModelDirectory(
                    null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String      suffix,
                    imp_targets); // imp_scenes); // ImagePlus   imp)
        }
        	// always generating UM, even if not saving - needed for targets
        String cuas_title = imp_targets.getTitle();
        String um_suffix = "";
        if (mono_fixed) {
        	um_suffix = String.format("-UM%.1f_%.3f_%.0f",um_sigma,um_weight,mono_range);
        } else {
        	um_suffix = String.format("-UM%.1f_%.3f_A",um_sigma,um_weight);
        }
        imp_targets = OpticalFlow.applyUM ( // apply UM
        		cuas_title + um_suffix , // final String title, // should include -UM...
        		imp_targets,                  // final ImagePlus imp,
        		um_sigma,                  // final double um_sigma,
        		um_weight);                // final double um_weight)
        if (save_um_cuas) {
        	center_CLT.saveImagePlusInModelDirectory(
        			null,      // "GPU-SHIFTED-D"+clt_parameters.disparity, // String      suffix,
        			imp_targets); // imp_scenes); // ImagePlus   imp)
        }
		
        boolean insert_average = (center_CLT.getCenterAverage() != null) || clt_parameters.imp.calculate_average;
        System.out.println("Will generate targets images/videos");
        int first_corr = insert_average? 1:0; // skip average
        int num_scenes = imp_targets.getStack().getSize()- first_corr; // includes average
        float [][] fpixels = new float[num_scenes][];
        String [] scene_titles = new String [num_scenes];
        for (int nscene = 0; nscene < fpixels.length; nscene++) {
            fpixels[nscene] = (float[]) imp_targets.getStack().getPixels(nscene+first_corr+1);
            String s = imp_targets.getStack().getSliceLabel(nscene+first_corr+1);
            if (s.indexOf("-0") >=0) {
                s=s.substring(0, s.indexOf("-0"));
            }
            scene_titles[nscene] = s;// imp_targets.getStack().getSliceLabel(nscene+first_corr+1);
        }
        boolean skip_targets = false;// true;
        if (skip_targets) {
        	return null;
        }
		this.cuasMotion = new CuasMotion (
				clt_parameters, // CLTParameters     clt_parameters,
				scene_titles,   // String []         scene_titles,
				center_CLT,     // QuadCLT           parentCLT,
				uasLogReader,   // UasLogReader      uasLogReader,
				debugLevel);    // int               debugLevel)
        
		cuasMotion.processMovingTargetsMulti(
				batch_mode,     // final boolean         batch_mode,
				fpixels,        // final float [][]      fpixels,
				debugLevel);    // final int             debugLevel) {
		double[][][] targets = cuasMotion.getTargets();
		double [][][] extended_targets = CuasMotion.extendMotionScan(
				targets, // final double [][][] motion_scan,
				null,                 // filter5,      // final boolean [][]  filtered, // centers, should be non-overlapped
				cuasMotion.getTilesX(),    // final int           tilesX)
				2,                    // final int           range, // 1 or 2
				null); // remain);              // final int     []    remain)
		if (rng_img) {
			if (debugLevel > -4) {
				System.out.println("detectTargets(): Generating and saving per-sensor target images");
			}
			double [][][] rendered_keyframes =  renderKeyFrames(
					targets,          //final double [][][] targets,   // centers
					extended_targets, // final double [][][] targets5x5,
					debugLevel);      // final int           debugLevel)
			boolean show = false; // true;
			int num_sens =   center_CLT.getNumSensors();
			String [] sens_titles = new String[num_sens+1];
			for (int i = 0; i < num_sens; i++) {
				sens_titles[i] = "SENS-"+i;
			}
			sens_titles[num_sens] = "COMBO";
			ImagePlus imp_rendered = ShowDoubleFloatArrays.showArraysHyperstack(
					rendered_keyframes,       // double[][][] pixels, 
					center_CLT.getWidth(),         // int          width, 
					center_CLT.getImageName()+"-RENDERED_TARGETS-R"+rng_radius+"-B"+rng_blur+"-UM"+rng_um_sigma,          // String       title, "time_derivs-rt"+diff_time_rt+"-rxy"+diff_time_rxy,
					sens_titles,         // String []    titles, // all slices*frames titles or just slice titles or null
					cuasMotion.getSliceTitles(), // CuasMotionLMA.LMA_TITLES,  // String []    frame_titles, // frame titles or null
					show);          // boolean      show)
			center_CLT.saveImagePlusInModelDirectory(imp_rendered);            // ImagePlus   imp)
		}
		return cuasMotion;
	}
	
	public double [][][] renderKeyFrames(
			final double [][][] targets,   // centers
			final double [][][] targets5x5,
			final int           debugLevel) {
		int num_seq = cuasMotion.getNumCorrSamples();
		double [][][] render_targets = new double [num_seq][][];
		for (int nseq = 0; nseq < num_seq; nseq++) {
			render_targets[nseq] = renderKeyFrame(
					targets,     // final double [][][] targets,   // centers
					targets5x5,  // final double [][][] targets5x5,
					nseq);       // final int           nseq)
		}
		return render_targets;
	}
	
	/**
	 * Preparation for ranging - rendering per-pixel, around targets (now 5x5 or 3x3 tiles), separate for each sensor
	 * @param targets5x5 targets, expanded around each detected target (will not be needed for ranging)
	 * @param radius radius of the mask around the target for each sensor individually
	 * @param nseq   number of sequence (keyframe)
	 * @return rendered images for each sensor separately [sensor][pixel]
	 */
	public double [][] renderKeyFrame(
			final double [][][] targets,   // centers
			final double [][][] targets5x5,
			final int           nseq) {
		final boolean um_en =          clt_parameters.imp.cuas_rng_um;
		final boolean um_all =         clt_parameters.imp.cuas_rng_um_all;
		final double um_sigma =        clt_parameters.imp.cuas_rng_um_sigma;
		final double um_weight =       clt_parameters.imp.cuas_rng_um_weight;
//		final double radius =          clt_parameters.imp.cuas_rng_radius;
//		final double radius_blur =     clt_parameters.imp.cuas_rng_blur;

		final boolean mb_en =       clt_parameters.imp.mb_en; //  && (fov_tiles==null) && (mode3d > 0);
		final double  mb_tau =      clt_parameters.imp.mb_tau;      // 0.008; // time constant, sec
		final 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
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);

		final int frame_center =     cuasMotion.getFrameCenter(nseq);
		final int half_accum_range = cuasMotion.getSeqLength()/2;
		//		final boolean   unsharped 


		final double [] window_full= cuasMotion.getSegmentWindow(
				true); // boolean smooth)
		//radius_blur
		final int start_scene = frame_center - half_accum_range;
		final int num_scenes = 2*half_accum_range + 1;
		final double [][][] img_um_seq = QuadCLT.unsharpMaskSourceMono(
				scenes,      // final QuadCLT[] scenes,
				start_scene, // final int       start_scene,
				num_scenes,  // final int       num_scenes,
				um_en,       // final boolean   um_en,
				um_all,      // final boolean   unsharped,
				um_sigma,    // final double    um_sigma,
				um_weight,   // final double    um_weight,
				debugLevel); // final int       debugLevel)
		double [][][] pXpYD5x5s = cuasMotion.targetPxPyD(
				targets5x5[nseq]); // final double [][] targets)
		double [][][] pXpYDs = cuasMotion.targetPxPyD(
				targets[nseq]); // final double [][] targets)

		///   	boolean batch_run =clt_parameters.batch_run; // may be modified for debug
		///		boolean cuas_debug =  clt_parameters.imp.cuas_debug;  // save debug images (and show them if not in batch mode)
		final int tilesX =     center_CLT.getTilesX();
		final int tilesY =     center_CLT.getTilesY();
		final int tileSize =   center_CLT.getTileSize();
		final int sensor_mask = -1; // all sensors
		final int num_sens =   center_CLT.getNumSensors();
		final int width = tilesX * tileSize;
		final int height = tilesY * tileSize;
		ErsCorrection ers_reference = center_CLT.getErsCorrection();
		int num_used_sens = 0;
		for (int i = 0; i < num_sens; i++) if (((sensor_mask >> i) & 1) != 0) num_used_sens++;
		int [] channels = new int [num_used_sens];
		int nch = 0;
		for (int i = 0; i < num_sens; i++) if (((sensor_mask >> i) & 1) != 0) channels[nch++] = i;
		double [][] rendered = new double [num_sens + 1][width*height];
		for (int dseq = half_accum_range; dseq <= half_accum_range; dseq++) {
			final double [][] ref_pXpYD5x5 = pXpYD5x5s[dseq + half_accum_range];
			final double [][] ref_pXpYD =    pXpYDs[dseq + half_accum_range]; // center target
			
			final int nscene = frame_center + dseq;
			final int sm = -1; // merge_all? -1: sensor_mask;
			double [][] drender;
			final double [][] dxyzatr_dt = (mb_en ? new double[][] { // for all, including ref
				scenes[nscene].getErsCorrection().getErsXYZ_dt(),
				scenes[nscene].getErsCorrection().getErsATR_dt()} : null);
			final String ts = scenes[nscene].getImageName();
			final double [] scene_xyz = ers_reference.getSceneXYZ(ts);
			final double [] scene_atr = ers_reference.getSceneATR(ts);
			if ((scene_atr==null) || (scene_xyz == null)) {  // should not happen
				System.out.println("renderKeyFrame() BUG1");
				continue;
			}
			prepareAltImages(
					ref_pXpYD,   // final double [][]   ref_pXpYD,
					img_um_seq,  // final double [][][] img_um_seq,
					scene_xyz,   // final double []     scene_xyz,       
					scene_atr,   // final double []     scene_atr,       
					nseq,        // final int           nseq,
					dseq,        // final int           dseq,
					debugLevel); // final int           debugLevel)

			
			if (mb_en && (dxyzatr_dt != null)) {
				double [][] motion_blur = OpticalFlow.getMotionBlur(
						center_CLT, // quadCLTs[ref_index],   // QuadCLT        ref_scene,
						scenes[nscene],        // QuadCLT        scene,         // can be the same as ref_scene
						ref_pXpYD5x5,             // 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)
				// TODO: Add target movement (recalculate velocities) 

				drender = QuadCLT.renderDoubleGPUFromDSI(
						sm,                  // final int         sensor_mask,
						false, // merge_all,           // final boolean     merge_channels,
						null,                // final Rectangle   full_woi_in,      // show larger than sensor WOI (or null)
						clt_parameters,      // CLTParameters     clt_parameters,
						null,                // ref_disparity,       // double []         disparity_ref,
						ref_pXpYD5x5,           // 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
						scenes[nscene],    // final QuadCLT     scene,
						center_CLT, // quadCLTs[ref_index], // final QuadCLT     ref_scene, // now - may be null - for testing if scene is rotated ref
						false,               // final boolean     toRGB,
						clt_parameters.imp.show_mono_nan,
						debugLevel);         // int         debugLevel)
			} else {
				drender = QuadCLT.renderDoubleGPUFromDSI(
						sm,                  // final int         sensor_mask,
						false, // merge_all,           // final boolean     merge_channels,
						null, // fov_tiles,           // testr, // null,                // final Rectangle   full_woi_in,      // show larger than sensor WOI (or null)
						clt_parameters,      // CLTParameters     clt_parameters,
						null, // ref_disparity,       // double []         disparity_ref,
						ref_pXpYD5x5,   // double [][]       ref_pXpYD,    // alternative to disparity_ref when reference is not uniform
						0.0,              // double            mb_tau,      // 0.008; // time constant, sec
						0.0,         // double            mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
						null,         // double [][]       mb_vectors,  //
						// not used, just as null/not null now
						// null means uniform grid, no view transform. even with 0 rot ERS was changing results
						scene_xyz,           // final double []   scene_xyz, // camera center in world coordinates
						scene_atr,           // final double []   scene_atr, // camera orientation relative to world frame
						scenes[nscene],    // final QuadCLT     scene,
						center_CLT, // quadCLTs[ref_index], // final QuadCLT     ref_scene, // now - may be null - for testing if scene is rotated ref
						false,               // final boolean     toRGB,
						clt_parameters.imp.show_mono_nan,
						debugLevel);         // int         debugLevel)
			}
			ai.set(0);
			final double scale = window_full[half_accum_range + dseq];
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					public void run() {
						for (int nPixY = ai.getAndIncrement(); nPixY < height; nPixY = ai.getAndIncrement()) {
							for (int npixX = 0; npixX < width; npixX++) {
								int npix = npixX + width* nPixY;
								if (!Double.isNaN(rendered[0][npix])) {
									for (int nchn = 0; nchn < num_sens; nchn++) {
										rendered[nchn][npix] += scale * drender[nchn][npix];
									}
								}
							}
						}
					}
				};
			}		      
			ImageDtt.startAndJoin(threads);
		}
		Arrays.fill(rendered[num_sens], Double.NaN);
		ai.set(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nPixY = ai.getAndIncrement(); nPixY < height; nPixY = ai.getAndIncrement()) {
						for (int npixX = 0; npixX < width; npixX++) {
							int npix = npixX + width* nPixY;
							if (!Double.isNaN(rendered[0][npix])) {
								double d = 0;
								for (int nchn = 0; nchn < num_sens; nchn++) {
									d += rendered[nchn][npix];
								}
								rendered[num_sens][npix] = d/num_sens;
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return rendered;
	}
	

	public double [][] refineTargetDisparity(
			final double [][][] targets,   // centers
			final double [][][] targets5x5,
			final int           nseq) {
		final boolean um_en =       clt_parameters.imp.cuas_rng_um;
		final boolean um_all =      clt_parameters.imp.cuas_rng_um_all;
		final double um_sigma =     clt_parameters.imp.cuas_rng_um_sigma;
		final double um_weight =    clt_parameters.imp.cuas_rng_um_weight;
//		final double radius =       clt_parameters.imp.cuas_rng_radius;
//		final double radius_blur =  clt_parameters.imp.cuas_rng_blur;
		final boolean mb_en =       clt_parameters.imp.mb_en; //  && (fov_tiles==null) && (mode3d > 0);
		final double  mb_tau =      clt_parameters.imp.mb_tau;      // 0.008; // time constant, sec
		final 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
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);

		final int frame_center =     cuasMotion.getFrameCenter(nseq);
		final int half_accum_range = cuasMotion.getSeqLength()/2;
		//		final boolean   unsharped 


		final double [] window_full= cuasMotion.getSegmentWindow(
				true); // boolean smooth)
		//radius_blur
		final int start_scene = frame_center - half_accum_range;
		final int num_scenes = 2*half_accum_range + 1;
		final double [][][] img_um_seq = QuadCLT.unsharpMaskSourceMono(
				scenes,      // final QuadCLT[] scenes,
				start_scene, // final int       start_scene,
				num_scenes,  // final int       num_scenes,
				um_en,       // final boolean   um_en,
				um_all,      // final boolean   unsharped,
				um_sigma,    // final double    um_sigma,
				um_weight,   // final double    um_weight,
				debugLevel); // final int       debugLevel)
		double [][][] pXpYD5x5s = cuasMotion.targetPxPyD(
				targets5x5[nseq]); // final double [][] targets)
		double [][][] pXpYDs = cuasMotion.targetPxPyD(
				targets[nseq]); // final double [][] targets)

		///   	boolean batch_run =clt_parameters.batch_run; // may be modified for debug
		///		boolean cuas_debug =  clt_parameters.imp.cuas_debug;  // save debug images (and show them if not in batch mode)
		final int tilesX =     center_CLT.getTilesX();
		final int tilesY =     center_CLT.getTilesY();
		final int tileSize =   center_CLT.getTileSize();
		final int sensor_mask = -1; // all sensors
		final int num_sens =   center_CLT.getNumSensors();
		final int width = tilesX * tileSize;
		final int height = tilesY * tileSize;
		ErsCorrection ers_reference = center_CLT.getErsCorrection();
		int num_used_sens = 0;
		for (int i = 0; i < num_sens; i++) if (((sensor_mask >> i) & 1) != 0) num_used_sens++;
		int [] channels = new int [num_used_sens];
		int nch = 0;
		for (int i = 0; i < num_sens; i++) if (((sensor_mask >> i) & 1) != 0) channels[nch++] = i;
		double [][] rendered = new double [num_sens + 1][width*height];
		for (int dseq = half_accum_range; dseq <= half_accum_range; dseq++) {
			final double [][] ref_pXpYD5x5 = pXpYD5x5s[dseq + half_accum_range];
			final double [][] ref_pXpYD =    pXpYDs[dseq + half_accum_range]; // center target
			final int nscene = frame_center + dseq;
			final int sm = -1; // merge_all? -1: sensor_mask;
			double [][] drender;
			final double [][] dxyzatr_dt = (mb_en ? new double[][] { // for all, including ref
				scenes[nscene].getErsCorrection().getErsXYZ_dt(),
				scenes[nscene].getErsCorrection().getErsATR_dt()} : null);
			final String ts = scenes[nscene].getImageName();
			final double [] scene_xyz = ers_reference.getSceneXYZ(ts);
			final double [] scene_atr = ers_reference.getSceneATR(ts);
			if ((scene_atr==null) || (scene_xyz == null)) {  // should not happen
				System.out.println("renderKeyFrame() BUG1");
				continue;
			}
			prepareAltImages(
					ref_pXpYD,   // final double [][]   ref_pXpYD,
					img_um_seq,  // final double [][][] img_um_seq,
					scene_xyz,   // final double []     scene_xyz,       
					scene_atr,   // final double []     scene_atr,       
					nseq,        // final int           nseq,
					dseq,        // final int           dseq,
					debugLevel); // final int           debugLevel)
			
			if (mb_en && (dxyzatr_dt != null)) {
				double [][] motion_blur = OpticalFlow.getMotionBlur(
						center_CLT, // quadCLTs[ref_index],   // QuadCLT        ref_scene,
						scenes[nscene],        // QuadCLT        scene,         // can be the same as ref_scene
						ref_pXpYD5x5,             // 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)
				// TODO: Add target movement (recalculate velocities) 

				drender = QuadCLT.renderDoubleGPUFromDSI(
						sm,                  // final int         sensor_mask,
						false, // merge_all,           // final boolean     merge_channels,
						null,                // final Rectangle   full_woi_in,      // show larger than sensor WOI (or null)
						clt_parameters,      // CLTParameters     clt_parameters,
						null,                // ref_disparity,       // double []         disparity_ref,
						ref_pXpYD5x5,           // 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
						scenes[nscene],    // final QuadCLT     scene,
						center_CLT, // quadCLTs[ref_index], // final QuadCLT     ref_scene, // now - may be null - for testing if scene is rotated ref
						false,               // final boolean     toRGB,
						clt_parameters.imp.show_mono_nan,
						debugLevel);         // int         debugLevel)
			} else {
				drender = QuadCLT.renderDoubleGPUFromDSI(
						sm,                  // final int         sensor_mask,
						false, // merge_all,           // final boolean     merge_channels,
						null, // fov_tiles,           // testr, // null,                // final Rectangle   full_woi_in,      // show larger than sensor WOI (or null)
						clt_parameters,      // CLTParameters     clt_parameters,
						null, // ref_disparity,       // double []         disparity_ref,
						ref_pXpYD5x5,   // double [][]       ref_pXpYD,    // alternative to disparity_ref when reference is not uniform
						0.0,              // double            mb_tau,      // 0.008; // time constant, sec
						0.0,         // double            mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
						null,         // double [][]       mb_vectors,  //
						// not used, just as null/not null now
						// null means uniform grid, no view transform. even with 0 rot ERS was changing results
						scene_xyz,           // final double []   scene_xyz, // camera center in world coordinates
						scene_atr,           // final double []   scene_atr, // camera orientation relative to world frame
						scenes[nscene],    // final QuadCLT     scene,
						center_CLT, // quadCLTs[ref_index], // final QuadCLT     ref_scene, // now - may be null - for testing if scene is rotated ref
						false,               // final boolean     toRGB,
						clt_parameters.imp.show_mono_nan,
						debugLevel);         // int         debugLevel)
			}
			ai.set(0);
			final double scale = window_full[half_accum_range + dseq];
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					public void run() {
						for (int nPixY = ai.getAndIncrement(); nPixY < height; nPixY = ai.getAndIncrement()) {
							for (int npixX = 0; npixX < width; npixX++) {
								int npix = npixX + width* nPixY;
								if (!Double.isNaN(rendered[0][npix])) {
									for (int nchn = 0; nchn < num_sens; nchn++) {
										rendered[nchn][npix] += scale * drender[nchn][npix];
									}
								}
							}
						}
					}
				};
			}		      
			ImageDtt.startAndJoin(threads);
		}
		Arrays.fill(rendered[num_sens], Double.NaN);
		ai.set(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nPixY = ai.getAndIncrement(); nPixY < height; nPixY = ai.getAndIncrement()) {
						for (int npixX = 0; npixX < width; npixX++) {
							int npix = npixX + width* nPixY;
							if (!Double.isNaN(rendered[0][npix])) {
								double d = 0;
								for (int nchn = 0; nchn < num_sens; nchn++) {
									d += rendered[nchn][npix];
								}
								rendered[num_sens][npix] = d/num_sens;
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return rendered;
	}
	
	public void prepareAltImages(
			final double [][]   ref_pXpYD,
			final double [][][] img_um_seq,
			final double []     scene_xyz,       
			final double []     scene_atr,       
			final int           nseq,
			final int           dseq,
			final int           debugLevel) {
		final double radius =       clt_parameters.imp.cuas_rng_radius;
		final double radius_blur =  clt_parameters.imp.cuas_rng_blur;
		final int tilesX =     center_CLT.getTilesX();
		final int tileSize =   center_CLT.getTileSize();
		final int width = tilesX * tileSize;
		final int frame_center =      cuasMotion.getFrameCenter(nseq);
		final int half_accum_range =  cuasMotion.getSeqLength()/2;
		ErsCorrection ers_reference = center_CLT.getErsCorrection();
		final int num_sens =          center_CLT.getNumSensors();

		final int nscene = frame_center + dseq;
		final int iscene = half_accum_range + dseq;
		
		final String ts = scenes[nscene].getImageName();
		final double []   scene_ers_xyz_dt = ers_reference.getSceneErsXYZ_dt(ts);
		final double []   scene_ers_atr_dt = ers_reference.getSceneErsATR_dt(ts);
		scenes[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 radius >0, apply mask (around getSensorsXY()). If 0 - bypass
		double [][][] img_sens_col = new double [num_sens][1][];
		if (radius > 0) {
			double [][][] sensors_xy = getSensorsXY(  //[tile][sensor]{x,y}
					clt_parameters, // CLTParameters     clt_parameters,
					ref_pXpYD,          // double [][]       ref_pXpYD,    // alternative to disparity_ref when reference is not uniform
					scene_xyz,          // double []         scene_xyz, // camera center in world coordinates. If null - no shift, no ers
					scene_atr,          // double []         scene_atr, // camera orientation relative to world frame
					scenes[nscene],     // final QuadCLT     scene,
					center_CLT,         // final QuadCLT     ref_scene, // now - may be null - for testing if scene is rotated ref
					debugLevel);        // final int         debugLevel);
			double [][] masked = copyMaskedTargets ( // [sensor][pixel]
					radius,             // final double        radius,
					radius_blur,        // final double        radius_blur,
					img_um_seq[iscene], // final double [][]   channel_img,
					sensors_xy,         // final double [][][] sensors_xy, // [tile][sensor]{x,y}
					width,              // final int           width,
					debugLevel);        // final int           debugLevel);
			for (int nsens = 0; nsens < num_sens; nsens++) {
				img_sens_col[nsens][0] = masked[nsens];
			}
		} else {
			for (int nsens = 0; nsens < num_sens; nsens++) {
				img_sens_col[nsens][0] = img_um_seq[iscene][nsens]; // Index 80 out of bounds for length 71
			}
		}
		scenes[nscene].setImageDataAlt(img_sens_col);
		return; //  true;	
	}
	
	
	
	public static double [][][] getSensorsXY( // will return PxPyD
			CLTParameters     clt_parameters,
			double [][]       ref_pXpYD,    // alternative to disparity_ref when reference is not uniform
  			// motion blur compensation 
			double []         scene_xyz, // camera center in world coordinates. If null - no shift, no ers
			double []         scene_atr, // camera orientation relative to world frame
			final QuadCLT     scene,
			final QuadCLT     ref_scene, // now - may be null - for testing if scene is rotated ref
			final int         debugLevel){
		double [][] pXpYD;
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
		// window in pixels!
			pXpYD=OpticalFlow.transformToScenePxPyD(
					ref_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
					ref_scene, // final QuadCLT     reference_QuadClt)
					scene);    // final QuadCLT     scene_QuadClt)
			scene.getErsCorrection().setupERS(); // NEW - did not help
//			ref_scene.getErsCorrection().setupERS(); // just in case - setup using instance parameters - inside
		
		int rendered_width = scene.getErsCorrection().getSensorWH()[0];
		final TpTask[] tp_tasks =  GpuQuad.setInterTasks( // "true" reference, with stereo actual reference will be offset
                scene.getNumSensors(),
                rendered_width,           // should match output size, pXpYD.length
                !scene.hasGPU(),          // final boolean             calcPortsCoordinatesAndDerivatives, // GPU can calculate them centreXY
                pXpYD,                    // final double [][]         pXpYD, // per-tile array of pX,pY,disparity triplets (or nulls)
                null,                     // final boolean []          selection, // may be null, if not null do not  process unselected tiles
                scene.getErsCorrection(), // final GeometryCorrection  geometryCorrection,
                clt_parameters.imp.disparity_corr, // 04/07/2023 // 0.0,                      // final double              disparity_corr,
                -1, // 0, // margin,      // final int                 margin,      // do not use tiles if their centers are closer to the edges
                null,                     // final boolean []          valid_tiles,            
                ImageDtt.THREADS_MAX);              // final int                 threadsMax)  // maximal number of threads to launch
//	    scene.saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
	    ImageDtt image_dtt = new ImageDtt(
	    		scene.getNumSensors(),
	    		clt_parameters.transform_size,
	    		clt_parameters.img_dtt,
	    		scene.isAux(),
	    		scene.isMonochrome(),
	    		scene.isLwir(),
	    		clt_parameters.getScaleStrength(scene.isAux()),
	    		scene.getGPU());
//	    boolean use_reference = false;
	    image_dtt.preSetReferenceTD( // do not run execConvertDirect, exit after updating tasks
	    		clt_parameters.img_dtt,  // ,     // final ImageDttParameters  imgdtt_params,    // Now just extra correlation parameters, later will include, most others
				tp_tasks,                // final TpTask[]            tp_tasks,
				debugLevel);             // final int                 globalDebugLevel)
	    final double [][][] sensors_xy = new double [tp_tasks.length][][];
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nTile= ai.getAndIncrement(); nTile < tp_tasks.length; nTile = ai.getAndIncrement()) if ((tp_tasks[nTile] != null) && (tp_tasks[nTile].getTask() != 0)){
						sensors_xy[nTile] = tp_tasks[nTile].getDoubleXY();
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
	    return sensors_xy;
	}
	public static double [][]  copyMaskedTargets ( // [sensor][pixel]
			final double        radius,
			final double        radius_blur,
			final double [][]   channel_img,
			final double [][][] sensors_xy, // [tile][sensor]{x,y}
			final int           width,
			final int           debugLevel){
		final int num_sens =  channel_img.length;
		final int num_pix =   channel_img[0].length;
		final int num_tiles = sensors_xy.length;
		final int height = channel_img[0].length / width;
		final double [][] masked_img = new double [num_sens][num_pix];
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
		final double inner_radius = radius * (1.0 - radius_blur);
		final double outer_radius = radius * (1.0 + radius_blur);
		final double inner_radius2 = inner_radius * inner_radius;
		final double outer_radius2 = outer_radius * outer_radius;
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nTile= ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) if (sensors_xy[nTile] != null) {
						double [][] sens_xy = sensors_xy[nTile];
						for (int nsens = 0; nsens < num_sens; nsens++) {
							double xc = sens_xy[nsens][0];
							double yc = sens_xy[nsens][1];
							int py0 = Math.max((int) Math.floor(yc - outer_radius), 0);
							int py1 = Math.min((int) Math.ceil (yc + outer_radius), height-1);
							int px0 = Math.max((int) Math.floor(xc - outer_radius), 0);
							int px1 = Math.min((int) Math.ceil (xc + outer_radius), width-1);
							for (int py = py0; py <= py1; py++) {
								double dy = py - yc;
								double dy2 = dy*dy;
								for (int px = px0; px <= px1; px++) {
									int npix = py*width + px;
									double dx = px - xc;
									double r2 = dy2+dx*dx;
									if (r2 <= inner_radius2) {
										masked_img[nsens][npix] = channel_img[nsens][npix];
									} else if (r2 <= outer_radius2) {
										double r = Math.sqrt(r2);
										double k =  0.5 * (1.0 + Math.cos(Math.PI*(r - inner_radius) / (outer_radius - inner_radius)));
										masked_img[nsens][npix] = k* channel_img[nsens][npix];
									}
								}
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return masked_img;
	}
	

}
