package com.elphel.imagej.vegetation;

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

import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.tileprocessor.ImageDtt;
import com.elphel.imagej.tileprocessor.QuadCLT;

import Jama.Matrix;

public class VegetationLMA {
	public static final int TVAO_TERRAIN =          0;
	public static final int TVAO_VEGETATION =       1;
	public static final int TVAO_VEGETATION_ALPHA = 2;
	public static final int TVAO_SCENE_OFFSET =     3;

	public final Rectangle    full;
//	public final int          image_width;
//	public final int          image_height;
	public final int          image_length;
	public final int          num_scenes;
	public Rectangle          woi =             null;
	public double []          vegetation_average; // full image average vegetation (with nulls)
	public double []          terrain_average;    // full image average terrain (no nulls)
	public double [][]        terrain_rendered;   // terrain rendered for scenes (has nulls)
	public double [][][]      vegetation_offsets; // [num_scenes][pixle]{dx,dy} differential offsets of vegetation to terrain grid
	public boolean            diff_offsets;
	// next 3 arrays are full [image_width*image_height], but will be modified only inside woi
//	public double []          terrain;            // 
//	public double []          vegetation;            // 
//	public double []          alpha;            // 
	public double [][]        tvao;            // [0][image_length] - terrain, [1][image_length] - vegetation,
//[2][image_length] - alpha, [3][num_scenes] - per-scene offset (mostly for vignetting. MAYBE add per-scene pair of tilts
	public int [][]           par_index;       // indices -  [0..3][full_pixel] - same as for tvao, value - parameter index
//	private double            default_alpha;
	private int               num_pars_terrain; 
	private int               num_pars_vegetation; 
	private int               num_pars_scenes;
	private int [][][]        data_source; // [index][windx][3]:
	                                       // 0:{ scene, full_indx, terr_indx, scale_indx} - always present as parameters
	                                       // 1: [4]/null Z-shape 4 corners vegetation, either >= as parameter index or -1 - (x + image_width*y) of the unmodified full index
	                                       // 2: [4]/null Z-shape 4 corners vegetation alpha, either >= as parameter index or -1 - (x + image_width*y) of the unmodified full index
	private double [][]       corners_weights; // matches data_source, non-nulls specify Z-shape 4 corners of vegetation/alpha weights 
	
	private double []         parameters_vector = null;
	private double []         y_vector = null;
	private double            weight_pure =     0;
	private double []         weights; // normalized so sum is 1.0 for all - samples and extra regularization
	public double             delta=            1e-5; // 7;
	
	
	
	
	public int []             indices;
	public int [][]           cpairs =          null;
	public int                num_pairs = 0;
	private double []         last_rms =        null; // {rms, rms_pure}, matching this.vector
	private double []         good_or_bad_rms = null; // just for diagnostics, to read last (failed) rms
	private double []         initial_rms =     null; // {rms, rms_pure}, first-calcualted rms
	private double [][]       qpairs = null; 
	private double []         last_ymfx =       null;
	private double [][]       last_jt =         null;
	
	public boolean            last3only =       false;  // true; // debug feature
	public boolean            debug_deriv = true;
	public int                debug_width =     12;
	public int                debug_decimals =  9;

	public VegetationLMA (
			int                width,
			boolean            diff_mode,
//			int height,
//			int scenes,
			double []          terrain_average,    // full image average terrain (no nulls!)
			double []          vegetation_average, // full image average vegetation (with nulls)
			double [][]        terrain_rendered,   // terrain rendered for scenes (has nulls)
			double [][][]      vegetation_offsets  // [num_scenes][pixel]{dx,dy} differential offsets of vegetation to terrain grid
			  ) {
		full = new Rectangle(0,0,width, terrain_average.length/width);
//		image_width =   width;
		image_length =  terrain_average.length;
//		image_height =  image_length/width;
		diff_offsets = diff_mode;
		num_scenes =   vegetation_offsets.length;
		this.vegetation_average = vegetation_average;
		this.terrain_average =    terrain_average;   
		this.terrain_rendered =   terrain_rendered;  
		this.vegetation_offsets = vegetation_offsets;
		tvao = new double[4][];
		tvao[TVAO_TERRAIN] =          this.terrain_average.clone();
		tvao[TVAO_VEGETATION] =       this.vegetation_average.clone();
		tvao[TVAO_VEGETATION_ALPHA] = new double [image_length]; // 0 - use terrain
		tvao[TVAO_SCENE_OFFSET] =     new double [num_scenes];
		
	}
	
	public int prepareLMA(
			final Rectangle          woi,
			final int                min_scenes, // minimal number of scenes (inside woi) vegetation pixel must influence
			final double             default_alpha,
			final double []          scene_weights, // null or per-scene weights (higher for larger offsets)?
			final int                debugLevel) {
		this.woi = woi;
//		this.default_alpha = default_alpha;
		int     min_scenes_uses = min_scenes;
		int     min_scenes_used = min_scenes * 4;
		boolean [][] valid_woi = filterValidWoi (
				min_scenes_uses, // int     min_scenes_uses,
				min_scenes_used, // int     min_scenes_used,
				debugLevel > 3); // boolean debug_img){ // 4x?
		setupParametersIndices(valid_woi);
		setupParametersVector (default_alpha); // areas where both terrain and vegetation are available		
		setupDataSource(); // condensed
		setupYVector();
		setupWeights(scene_weights);
		last_jt = new double [parameters_vector.length][];
		return weights.length;
	}
	
	
	
	
	
	
	private double [] getYminusFxWeighted(
			final double []   fx,
			final double []   rms_fp // null or [2]
			) {
		final Thread[]      threads =     ImageDtt.newThreadArray();
		final AtomicInteger ai =          new AtomicInteger(0);
		final AtomicInteger ati =         new AtomicInteger(0);
		final double []     wymfw =       new double [fx.length];
		double [] swd2 = new double[threads.length];
		for (int ithread = 0; ithread < threads.length; ithread++) { // first sum for pairs
			threads[ithread] = new Thread() {
				public void run() {
					int nthread =  ati.getAndIncrement();
					for (int n = ai.getAndIncrement(); n < y_vector.length; n = ai.getAndIncrement()) {
						double d = y_vector[n] - fx[n]; // - fx[n]; // +y_vector[i]
						double wd = d * weights[n];
						wymfw[n] = wd;
						swd2[nthread] += d * wd;
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		double s_rms_pure = 0;
		for (int n = 0; n < swd2.length; n++) {
			s_rms_pure += swd2[n];
		}
//		System.out.println("ai.get()="+ai.get());
		// important to set - after first cycle ai is left 16(number of threads) larger than number of cycles!
		// It is so, because it first increments, then tests if (n < num_pairs)
		ai.set(y_vector.length); // not yet used
		ati.set(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					int nthread =  ati.getAndIncrement();
					for (int n = ai.getAndIncrement(); n < fx.length; n = ai.getAndIncrement()) {
						double d = -fx[n]; // - fx[n]; // +y_vector[i]
						double wd = d * weights[n];
						wymfw[n] = wd;
						swd2[nthread] += d * wd;
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		
		double s_rms = 0; // start from scratch
		for (int n = 0; n < swd2.length; n++) {
			s_rms += swd2[n];
		}
		
		if (rms_fp != null) {
			rms_fp[0] = Math.sqrt(s_rms);
			rms_fp[1] = Math.sqrt(s_rms_pure/weight_pure);
		}
		return wymfw;
	}
	
	public int runLma( // <0 - failed, >=0 iteration number (1 - immediately)
			double lambda,           // 0.1
			double lambda_scale_good,// 0.5
			double lambda_scale_bad, // 8.0
			double lambda_max,       // 100
			double rms_diff,         // 0.001
			int    num_iter,         // 20
			boolean last_run,
			String dbg_prefix,
			int    debug_level)
	{
		boolean [] rslt = {false,false};
		this.last_rms = null; // remove?
		int iter = 0;
		if (dbg_prefix != null) {
//			 debugStateImage(dbg_prefix+"-initial");
		}
		for (iter = 0; iter < num_iter; iter++) {
			rslt =  lmaStep(
					lambda,
					rms_diff,
					debug_level);
			if (dbg_prefix != null) {
//				 debugStateImage(dbg_prefix+"-step_"+iter);
			}
			
			if (rslt == null) {
				return -1; // false; // need to check
			}
			if (debug_level > 1) {
				System.out.println("LMA step"+String.format("%3d",iter)+": {"+rslt[0]+","+rslt[1]+"} full RMS= "+good_or_bad_rms[0]+
						" ("+initial_rms[0]+"), pure RMS="+good_or_bad_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
			}
			if (rslt[1]) {
				break;
			}
			if (rslt[0]) { // good
				lambda *= lambda_scale_good;
			} else {
				lambda *= lambda_scale_bad;
				if (lambda > lambda_max) {
					break; // not used in lwir
				}
			}
		}
		if (rslt[0]) { // better
			if (iter >= num_iter) { // better, but num tries exceeded
				if (debug_level > 1) System.out.println("Step "+iter+": Improved, but number of steps exceeded maximal");
			} else {
				if (debug_level > 1) System.out.println("Step "+iter+": LMA: Success");
			}

		} else { // improved over initial ?
			if (last_rms[0] < initial_rms[0]) { // NaN
				rslt[0] = true;
				if (debug_level > 1) System.out.println("Step "+iter+": Failed to converge, but result improved over initial");
			} else {
				if (debug_level > 1) System.out.println("Step "+iter+": Failed to converge");
			}
		}
		boolean show_intermediate = true;
		if (show_intermediate && (debug_level > 0)) {
			System.out.println("LMA: full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
		}
		if (debug_level > 2){ 
			System.out.println("iteration="+iter);
		}
		if (debug_level > 0) {
			if ((debug_level > 1) ||  last_run) { // (iter == 1) || last_run) {
				if (!show_intermediate) {
					System.out.println("LMA: iter="+iter+",   full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
				}
			}
		}
		if ((debug_level > -2) && !rslt[0]) { // failed
			if ((debug_level > 1) || (iter == 1) || last_run) {
				System.out.println("LMA failed on iteration = "+iter);
			}
			System.out.println();
		}

		return rslt[0]? iter : -1;
	}

	private boolean [] lmaStep(
			double lambda,
			double rms_diff,
			int debug_level) {
		boolean [] rslt = {false,false};
		// maybe the following if() branch is not needed - already done in prepareLMA !
		if (this.last_rms == null) { //first time, need to calculate all (vector is valid)
			last_rms = new double[2];
			if (debug_level > 1) {
				System.out.println("lmaStep(): first step");
			}
			double [] fx = getFxDerivs(
					parameters_vector, // double []         vector,
					last_jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
					debug_level);      // final int         debug_level)
			last_ymfx = getYminusFxWeighted(
					fx, // final double []   fx,
					last_rms); // final double []   rms_fp // null or [2]
			this.initial_rms = this.last_rms.clone();
			this.good_or_bad_rms = this.last_rms.clone();

			if (debug_level > -1) { // temporary
				/*
				dbgYminusFxWeight(
						this.last_ymfx,
						this.weights,
						"Initial_y-fX_after_moving_objects");
                */
			}
			if (last_ymfx == null) {
				return null; // need to re-init/restart LMA
			}
			// TODO: Restore/implement
			if (debug_level > 4) {
				double    delta = this.delta;
			 	double delta_err= debugDerivs (
			 			parameters_vector, // double [] vector,
						delta,             // double    delta,
						debug_level, //int       debugLevel,
						false); // boolean   show_img)
				System.out.println("\nMaximal error = "+delta_err);
			}
		}
		if (debug_level > 3) { // 0) {
			double    delta =  this.delta; // 1E-3;
		 	double delta_err= debugDerivs (
		 			parameters_vector, // double [] vector,
					delta,             // double    delta,
					debug_level, //int       debugLevel,
					false); // boolean   show_img)
			System.out.println("\nMaximal error = "+delta_err);
			int [] wh = new int[2];
			double [] debug_image =  parametersImage (
					parameters_vector, // double [] vector,
					1,                // int       gap)
					wh);
			ShowDoubleFloatArrays.showArrays(
					debug_image,
					wh[0],
					wh[1],
					"parameters.vector");
		}
		
		
		Matrix y_minus_fx_weighted = new Matrix(this.last_ymfx, this.last_ymfx.length);

		Matrix wjtjlambda = new Matrix(getWJtJlambda(
				lambda, // *10, // temporary
				this.last_jt)); // double [][] jt) // null
		
		if (debug_level>4) {
			System.out.println("JtJ + lambda*diag(JtJ");
			wjtjlambda.print(18, 6);
		}
		Matrix jtjl_inv = null;
		try {
			jtjl_inv = wjtjlambda.inverse(); // check for errors
		} catch (RuntimeException e) {
			rslt[1] = true;
			if (debug_level > 0) {
				System.out.println("Singular Matrix!");
			}

			return rslt;
		}
		if (debug_level>4) {
			System.out.println("(JtJ + lambda*diag(JtJ).inv()");
			jtjl_inv.print(18, 6);
		}
//last_jt has NaNs
		Matrix jty = (new Matrix(this.last_jt)).times(y_minus_fx_weighted);
		if (debug_level>2) {
			System.out.println("Jt * (y-fx)");
			jty.print(18, 6);
		}
		
		
		Matrix mdelta = jtjl_inv.times(jty);
		if (debug_level>2) {
			System.out.println("mdelta");
			mdelta.print(18, 6);
		}

		double scale = 1.0;
		double []  delta =      mdelta.getColumnPackedCopy();
		double []  new_vector = parameters_vector.clone();
		for (int i = 0; i < parameters_vector.length; i++) {
			new_vector[i] += scale * delta[i];
		}
		
		double [] fx = getFxDerivs(
				new_vector, // double []         vector,
				last_jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
				debug_level);      // final int         debug_level)
		double [] rms = new double[2];
		last_ymfx = getYminusFxWeighted(
//				vector_XYS, // final double [][] vector_XYS,
				fx, // final double []   fx,
				rms); // final double []   rms_fp // null or [2]
		if (debug_level > 2) {
			/*
			dbgYminusFx(this.last_ymfx, "next y-fX");
			dbgXY(new_vector, "XY-correction");
			*/
		}

		if (last_ymfx == null) {
			return null; // need to re-init/restart LMA
		}

		this.good_or_bad_rms = rms.clone();
		if (rms[0] < this.last_rms[0]) { // improved
			rslt[0] = true;
			rslt[1] = rms[0] >=(this.last_rms[0] * (1.0 - rms_diff));
			this.last_rms = rms.clone();

			this.parameters_vector = new_vector.clone();
			if (debug_level > 3) {
				double [] debug_image =  parametersImage (
						parameters_vector, // double [] vector,
						1,                // int       gap)
						null);

				// print vectors in some format
				/*
				System.out.print("delta: "+corr_delta.toString()+"\n");
				System.out.print("New vector: "+new_vector.toString()+"\n");
				System.out.println();
				*/
			}
		} else { // worsened
			rslt[0] = false;
			rslt[1] = false; // do not know, caller will decide
			// restore state
			fx = getFxDerivs(
					parameters_vector, // double []         vector,
					last_jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
					debug_level);      // final int         debug_level)
			last_ymfx = getYminusFxWeighted(
					fx, // final double []   fx,
					this.last_rms); // final double []   rms_fp // null or [2]
			if (last_ymfx == null) {
				return null; // need to re-init/restart LMA
			}
			if (debug_level > 2) {
				/*
				 dbgJacobians(
							corr_vector, // GeometryCorrection.CorrVector corr_vector,
							1E-5, // double delta,
							true); //boolean graphic)
							*/
			}
		}
		return rslt;
	}
	
	
	
	private double [][] getWJtJlambda(
			final double      lambda,
			final double [][] jt)
	{
		final int num_pars = jt.length;
		final int num_pars2 = num_pars * num_pars;
		final int nup_points = jt[0].length;
		final double [][] wjtjl = new double [num_pars][num_pars];
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int indx = ai.getAndIncrement(); indx < num_pars2; indx = ai.getAndIncrement()) {
						int i = indx / num_pars;
						int j = indx % num_pars;
						if (j >= i) {
							double d = 0.0;
							for (int k = 0; k < nup_points; k++) {
								if (jt[i][k] != 0) {
									d+=0;
								}
								d += weights[k]*jt[i][k]*jt[j][k];
							}
							wjtjl[i][j] = d;
							if (i == j) {
								wjtjl[i][j] += d * lambda;
							} else {
								wjtjl[j][i] = d;
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return wjtjl;
	}
	
	
	public double [] getRms() {
		return last_rms;
		
	}

	public double [] getInitialRms() {
		return initial_rms;
	}	
	
	
	
	private double [] getFxDerivs(
			final double []   vector,
			final double [][] jt, // should be null or initialized with [vector.length][]
			final int         debug_level)
	{
		double [] fX = new double [weights.length]; // num_pairs + vector.length];
		if (jt != null) {
			for (int i = 0; i < jt.length; i++) {
				jt[i] = new double [weights.length]; // weights.length];
			}
		}
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					double [] vegetation = new double [4]; 
					double [] alpha =      new double [4];
					for (int n = ai.getAndIncrement(); n < weights.length; n = ai.getAndIncrement()) {
//						int nscene =   data_source[n][0][0];
//						int indx =     data_source[n][0][1];
						double terrain = vector[data_source[n][0][2]];
						double [] cw = corners_weights[n]; 
						double d; 
						int [] indx_vegetation = data_source[n][1]; 
						int [] indx_alpha =      data_source[n][2];
						double sum_v =0, sum_a =0;
						if (cw != null) {
							for (int i = 0; i < 4; i++ ) {
								int iv = indx_vegetation[i], ia=indx_alpha[i];
								vegetation[i] = cw[i] * ((iv >= 0) ? vector[iv]: tvao[TVAO_VEGETATION][-1-iv]);
								alpha[i] =      cw[i] * ((ia >= 0) ? vector[ia]: tvao[TVAO_VEGETATION_ALPHA][-1-ia]);
								sum_v += vegetation[i];
								sum_a += alpha[i];
							}
							d = terrain * (1.0 - sum_a) + sum_v * sum_a;
							if (jt != null) {
								jt[data_source[n][0][2]][n] = 1 - sum_a; // d/dterrain
								for (int i = 0; i < 4; i++ ) {
									if (indx_vegetation[i] >= 0) {
//										jt[data_source[n][1][indx_vegetation[i]]][n] = cw[i] * sum_a; // d/dvegetation[i]
										jt[data_source[n][1][i]][n] = cw[i] * sum_a; // d/dvegetation[i]
									}
									if (indx_alpha[i] >= 0) {
//										jt[data_source[n][2][indx_alpha[i]]][n] = cw[i] * (sum_v - terrain); // d/dalpha[i]
										jt[data_source[n][2][i]][n] = cw[i] * (sum_v - terrain); // d/dalpha[i]
									}
								}								
								
							}

						} else {
							d = terrain; 
							if (jt != null) {
								jt[data_source[n][0][2]][n] = 1;  // d/dterrain
							}
						}
						double scene_offs = vector[data_source[n][0][3]];
						fX[n] = d + scene_offs;
						if (jt != null) {
							jt[data_source[n][0][3]][n] = 1;
						}
					}
				}
			};
		}
		ImageDtt.startAndJoin(threads);
		return fX;		
	}
	
	private double [][] getFxDerivsDelta(
			double []         vector,
			final double      delta,
			final int         debug_level) {
		double [][] jt =  new double [vector.length][weights.length];
		for (int nv = 0; nv < vector.length; nv++) {
			double [] vpm = vector.clone();
			vpm[nv]+= 0.5*delta;
			double [] fx_p =  getFxDerivs(
					vpm,
					null, // final double [][] jt, // should be null or initialized with [vector.length][]
					debug_level);
			vpm[nv]-= delta;
			double [] fx_m =  getFxDerivs(
					vpm,
					null, // final double [][] jt, // should be null or initialized with [vector.length][]
					debug_level);
			for (int i = 0; i < weights.length; i++) if (weights[i] > 0) {
				jt[nv][i] = (fx_p[i]-fx_m[i])/delta;
			}
		}
		return jt;
	}

	private int [] getParamDebugIndices(
			int gap) {
		int woi_width3=woi.width*3 + 2 * gap;
		int num_scene_rows = (int) Math.ceil(1.0*par_index[3].length/woi_width3);
		int [] indices = new int [woi_width3 * (woi.height + gap+ num_scene_rows)];
		Arrays.fill(indices, -1);
		for (int h = 0; h < woi.height; h++) {
			for (int n = 0; n < 3; n++) {
				for (int w = 0; w < woi.width; w++) {
					int indx = (h + woi.y) * full.width + (w + woi.x);
					indices [(h * woi_width3) + (woi.width + gap) * n + w] = par_index[n][indx]; // h * woi.width + w]; 
				}
			}
		}
		for (int i = 0; i < par_index[3].length; i++) {
			indices [((woi.height + gap) * woi_width3) + i] = par_index[3][i];
		}
		/*
		double [] dbg_img = new double[indices.length];
		for (int i = 0; i <indices.length; i++) {
			dbg_img[i] = indices[i];
		}
		ShowDoubleFloatArrays.showArrays(
				dbg_img,
				woi_width3,
				indices.length/woi_width3,
				"parameters_indices");
		*/
		return indices;
	}
	
	public double [] parametersImage (
			double [] vector,
			int       gap,
			int []    wh) {
		int [] indices = getParamDebugIndices (gap);
		int woi_width3=woi.width*3 + 2 * gap;
		double [] dbg_img = new double [indices.length];
		for (int i = 0; i < indices.length; i++) {
			if (indices[i] < 0) {
				dbg_img[i] = Double.NaN;
			} else {
				dbg_img[i]= vector[indices[i]];
			}
		}
		if (wh != null) {
			wh[0] = woi_width3;
			wh[1] = indices.length/woi_width3;
		}
		ShowDoubleFloatArrays.showArrays(
				dbg_img,
				woi_width3,
				indices.length/woi_width3,
				"parameters.vector");
		
		return dbg_img;
	}

	
	public double debugDerivs (
			double [] vector,
			double    delta,
			int       debugLevel,
			boolean   show_img) {
		double  [][] jt_delta =  getFxDerivsDelta (
				vector, // double []         vector,
				delta,  // final double      delta,
				debugLevel); // final int         debug_level)
		double [][] jt = new double [jt_delta.length][];
		getFxDerivs(
				vector,      // final double []   vector,
				jt,          // final double [][] jt, // should be null or initialized with [vector.length][]
				debugLevel); // final int         debug_level)
		double [][] jt_diff = new double [jt_delta.length][jt_delta[0].length];
		double max_err=0;
		for (int n = 0; n < jt_diff.length; n++) {
			for (int w = 0; w < jt_diff[0].length; w++) {
				jt_diff[n][w] = jt[n][w] - jt_delta[n][w];
				max_err = Math.max(max_err, Math.abs(jt_diff[n][w]));
			}
		}
		System.out.println("delta = "+delta+", max_err = "+max_err);
		// think of visualization
		/*
		if (show_img) {
			String [] frame_titles = {"jt","jt_delta", "jt_diff"}; 
			double [][][] debug_img = new double [frame_titles.length][][];
		}
		*/
		return max_err;
	}
	
	
	private void setupWeights(final double [] scene_weights) {
		int extra_samples = 0; // in the future may be regularization
		double extra_weights = 0.0;
		final double [] sw =  (scene_weights != null) ? scene_weights : new double [num_scenes];
		if (scene_weights == null) {
			Arrays.fill (sw, 1.0);
		}
		weights = new double [data_source.length + extra_samples];
		double s = 0;
		for (int ny = 0; ny < data_source.length; ny++) {
			s += sw[data_source[ny][0][0]];
		}
		final double s0 = s;
		for (int i = 0; i < extra_samples; i++) {
			s+=extra_weights;
		}
		final double k = 1.0/s;
		weight_pure = s0/s;
		
		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 nw = ai.getAndIncrement(); nw < weights.length; nw = ai.getAndIncrement()) {
						if (nw  < data_source.length) {
							weights[nw] = sw[data_source[nw][0][0]] * k;
						} else {
							weights[nw] = extra_weights * k;
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
	}
	
	private void setupYVector() {
//		double [] 
		y_vector = new double [data_source.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 ny = ai.getAndIncrement(); ny < data_source.length; ny = ai.getAndIncrement()) {
						int nscene =   data_source[ny][0][0];
						int indx =     data_source[ny][0][1];
						y_vector[ny] = terrain_rendered[nscene][indx];
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return;
	}
	
	private void setupDataSource() {
		final int woi_length = woi.width*woi.height;
		final int [][][][] data_src = new int [num_scenes][woi_length][][];
		final double [][][] corn_w = new double [num_scenes][woi_length][]; // null where no vegetation
		final int [] scene_samples = new int [num_scenes];
		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 nScene = ai.getAndIncrement(); nScene < num_scenes; nScene = ai.getAndIncrement()) {
						for (int drow = 0; drow < woi.height; drow++) {
							int row = woi.y + drow;
							for (int dcol = 0; dcol < woi.width; dcol++) {
								int col = woi.x + dcol;	
								int windx =dcol + drow * woi.width;
								int indx = row*full.width + col;
								if (!Double.isNaN(terrain_rendered[nScene][indx]) && (par_index[TVAO_TERRAIN][indx] >= 0)) { //
									scene_samples[nScene]++;
									data_src[nScene][windx] = new int[3][];
									// maybe use windx instead of indx below?
									data_src[nScene][windx][0] = new int [] {nScene, indx, par_index[TVAO_TERRAIN][indx],par_index[TVAO_SCENE_OFFSET][nScene]};
									// next 3 not needed - they are already nulls
									data_src[nScene][windx][1] = null;
									data_src[nScene][windx][2] = null;
									corn_w[nScene][windx] = null;
									double [] veg_xy = vegetation_offsets[nScene][indx];
									if (veg_xy != null) {
										veg_xy = veg_xy.clone();
										if (diff_offsets) {
											veg_xy[0] += col;
											veg_xy[1] += row;
										}
										int x0 = (int) Math.floor(veg_xy[0]);
										int y0 = (int) Math.floor(veg_xy[1]);
										if (    full.contains(x0,  y0)   &&
												full.contains(x0+1,y0)   &&
												full.contains(x0,  y0+1) &&
												full.contains(x0+1,y0+1)) {

											double fx = veg_xy[0] - x0;
											double fy = veg_xy[1] - y0;
											corn_w[nScene][windx] = new double []{(1-fx)*(1-fy), fx*(1-fy), (1-fx)*fy, fx*fy};
											data_src[nScene][windx][1] = new int [4];
											data_src[nScene][windx][2] = new int [4];

											int veg_indx = x0+full.width*y0;
											int [] veg_indx4 = {veg_indx, veg_indx+1, veg_indx+full.width, veg_indx+full.width+1};
											for (int i = 0; i < 4; i++ ) {
												data_src[nScene][windx][1][i] = (par_index[TVAO_VEGETATION][veg_indx4[i]] >= 0)?
														par_index[TVAO_VEGETATION][veg_indx4[i]] : (-1-veg_indx4[i]);
												data_src[nScene][windx][2][i] = (par_index[TVAO_VEGETATION_ALPHA][veg_indx4[i]] >= 0)?
														par_index[TVAO_VEGETATION_ALPHA][veg_indx4[i]] : (-1-veg_indx4[i]);
											}
										}
									}
								}
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		// now condense removing nulls in data_src (cornw may still have nulls)
		int num_samples = 0;
		for (int nscene = 0; nscene < scene_samples.length; nscene++) {
			int num_prev = num_samples;
			num_samples+= scene_samples[nscene];
			scene_samples[nscene] = num_prev; // start index 
		}
		data_source = new int [num_samples][][];
		corners_weights = new double[num_samples][];
		ai.set(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nScene = ai.getAndIncrement(); nScene < num_scenes; nScene = ai.getAndIncrement()) {
						int out_indx = scene_samples[nScene];
						for (int windx = 0; windx < woi_length; windx++) {
							if (data_src[nScene][windx] != null) {
								corners_weights[out_indx] =   corn_w[nScene][windx];
								data_source    [out_indx++] = data_src[nScene][windx];
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
	}
	
	private void setupParametersVector(
			double default_alpha) { // areas where both terrain and vegetation are available
		for (int i = 0; i < tvao[TVAO_VEGETATION_ALPHA].length; i++) {
			if (!Double.isNaN(tvao[TVAO_VEGETATION][i])) {
				tvao[TVAO_VEGETATION_ALPHA][i] = default_alpha;
			}
		}
		for (int drow = 0; drow < woi.height; drow++) {
			int row = woi.y + drow;
			for (int dcol = 0; dcol < woi.width; dcol++) {
				int col = woi.x + dcol;	
//				int windx =dcol + drow * woi.width;
				int indx = row*full.width + col;
				if (par_index[TVAO_TERRAIN][indx] >= 0) {
					parameters_vector[par_index[TVAO_TERRAIN][indx]] =    tvao[TVAO_TERRAIN][indx];
				}
				if (par_index[TVAO_VEGETATION][indx] >= 0) {
					parameters_vector[par_index[TVAO_VEGETATION][indx]] = tvao[TVAO_VEGETATION][indx];
				}
				if (par_index[TVAO_VEGETATION_ALPHA][indx] >= 0) {
					parameters_vector[par_index[TVAO_VEGETATION_ALPHA][indx]] = default_alpha;
				}
			}
		}
		for (int i = 0; i <  par_index[TVAO_SCENE_OFFSET].length; i++) {
			parameters_vector[par_index[TVAO_SCENE_OFFSET][i]]= tvao[TVAO_SCENE_OFFSET][i];
		}
	}
	
	
	private void setupParametersIndices(boolean [][] valid_woi) {
		par_index = new int [4][];
		par_index[TVAO_TERRAIN] =          new int [image_length];
		par_index[TVAO_VEGETATION] =       new int [image_length];
		par_index[TVAO_VEGETATION_ALPHA] = new int [image_length];
		par_index[TVAO_SCENE_OFFSET] =     new int [num_scenes];
		Arrays.fill(par_index[TVAO_TERRAIN],          -1);
		Arrays.fill(par_index[TVAO_VEGETATION],       -1);
		Arrays.fill(par_index[TVAO_VEGETATION_ALPHA], -1);
		int par_num = 0;
		for (int drow = 0; drow < woi.height; drow++) {
			int row = woi.y + drow;
			for (int dcol = 0; dcol < woi.width; dcol++) {
				int col = woi.x + dcol;	
				int windx =dcol + drow * woi.width;
				if (valid_woi[0][windx]) {
					int indx = row*full.width + col;
					par_index[TVAO_TERRAIN][indx] = par_num++;
				}
			}
		}
		num_pars_terrain = par_num;
		for (int drow = 0; drow < woi.height; drow++) {
			int row = woi.y + drow;
			for (int dcol = 0; dcol < woi.width; dcol++) {
				int col = woi.x + dcol;	
				int windx =dcol + drow * woi.width;
				if (valid_woi[1][windx]) {
					int indx = row*full.width + col;
					par_index[TVAO_VEGETATION][indx] = par_num++;
				}
			}
		}
		num_pars_vegetation = par_num - num_pars_terrain;
		for (int drow = 0; drow < woi.height; drow++) {
			int row = woi.y + drow;
			for (int dcol = 0; dcol < woi.width; dcol++) {
				int col = woi.x + dcol;	
				int windx =dcol + drow * woi.width;
				if (valid_woi[1][windx]) {
					int indx = row*full.width + col;
					par_index[TVAO_VEGETATION_ALPHA][indx] = par_num++;
				}
			}
		}
		int num_pars_pixel = par_num;
		for (int i = 0; i < par_index[TVAO_SCENE_OFFSET].length; i++) {
			par_index[TVAO_SCENE_OFFSET][i] = par_num++;
		}
		num_pars_scenes = par_num - num_pars_pixel;
		parameters_vector = new double [par_num];
	}
	
	
	private boolean [][] filterValidWoi (
			int     min_scenes_uses,
			int     min_scenes_used,
			boolean debug_img){ // 4x?
		int max_iter = 100;
		boolean [][] valid_pix = new boolean [2][woi.width*woi.height];
		Arrays.fill(valid_pix[0],true);
		Arrays.fill(valid_pix[1],true);
		int [] num_valid = {valid_pix[0].length,valid_pix[1].length};
		int [] num_valid_prev =num_valid.clone();
		int [][] num_uses;
		for (int iter = 0; iter < max_iter; iter++) {
			num_uses = numVegUses(valid_pix);
			num_valid = new int[2];
			for (int i = 0; i < valid_pix[0].length; i++) {
				valid_pix[0][i] = num_uses[0][i] > min_scenes_uses;
				valid_pix[1][i] = num_uses[1][i] > min_scenes_used;
				if (valid_pix[0][i]) num_valid[0]++;
				if (valid_pix[1][i]) num_valid[1]++;
			}
			System.out.println("Iteration "+(iter+1)+", valid pixels: terrain "+num_valid[0]+", vegetation "+num_valid[1]+" (was "+num_valid_prev[0]+", "+num_valid_prev[1]+")");
			if ((num_valid[0] == num_valid_prev[0]) && (num_valid[1] == num_valid_prev[1])) {
				break;
			}
			num_valid_prev =num_valid.clone();
			if (debug_img) {
				double [][] debug_uses = new double [4][num_uses[0].length];
				double max = 0;
				for (int i = 0; i < num_uses[0].length; i++) {
					debug_uses[0][i] = num_uses[0][i];
					debug_uses[1][i] = num_uses[1][i] *0.25;
					max = Math.max(max, debug_uses[0][i]);
					max = Math.max(max, debug_uses[1][i]);
				}
				for (int i = 0; i < num_uses[0].length; i++) {
					debug_uses[2][i] = valid_pix[0][i]? max:0;
					debug_uses[3][i] = valid_pix[1][i]? max:0;
				}
			ShowDoubleFloatArrays.showArrays(
					debug_uses,
					woi.width,
					woi.height,
					true,
					"number_of_uses_valid-"+iter+".tiff",
					new String[] {"uses_veg","veg_used","valid_terr","valid_veg"});
			}
			
		}
		
		return valid_pix; // [0] - valid terrain for adjustment, [1] valid vegetation
	}
	
	/**
	 * In how many scenes this pixel depends on vegetation in all 4 corners around it
	 * @return {number of scenes where this pixel depends on vegetation, 4x number of scenes where this vegetation pixel influences composite image.
	 */
	private int [][] numVegUses(
			boolean [][] valid_vegetation) { // 0 this pixel has adjustable terrain (depends on enough vegetation), 1 - has adjustable vegetation
		final AtomicInteger [] auses_veg = new AtomicInteger[woi.width*woi.height]; // this pixel depends on vegetation
		final AtomicInteger [] aused_veg = new AtomicInteger[woi.width*woi.height]; // this vegetation pixel influences composite image
		for (int i = 0; i < auses_veg.length; i++) {
			auses_veg[i] = new AtomicInteger(0);
			aused_veg[i] = new AtomicInteger(0);
		}
		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 nScene = ai.getAndIncrement(); nScene < num_scenes; nScene = ai.getAndIncrement()) {
						for (int drow = 0; drow < woi.height; drow++) {
							int row = woi.y + drow;
							for (int dcol = 0; dcol < woi.width; dcol++) {
								int col = woi.x + dcol;	
								int windx =dcol + drow * woi.width;
								int indx = row*full.width + col;
								if (!Double.isNaN(terrain_rendered[nScene][indx]) && ((valid_vegetation== null) || valid_vegetation[0][windx])) {
//									int indx = row*full.width + col;
									double [] veg_xy = vegetation_offsets[nScene][indx];
									if (veg_xy != null) {
										veg_xy = veg_xy.clone();
										if (diff_offsets) {
											veg_xy[0] += col;
											veg_xy[1] += row;
										}
										int x0 = (int) Math.floor(veg_xy[0]);
										int y0 = (int) Math.floor(veg_xy[1]);
										if (woi.contains(x0, y0) && woi.contains(x0+1, y0) && woi.contains(x0, y0+1) && woi.contains(x0+1,y0+1)) {
											if     ((vegetation_offsets[nScene][indx + 1] != null) &&
													(vegetation_offsets[nScene][indx + full.width] != null) &&
													(vegetation_offsets[nScene][indx + full.width + 1] != null)) {
												int veg_dindx0 = (x0-woi.x) + (y0-woi.y)*woi.width;
												if ((valid_vegetation== null) ||
														(		valid_vegetation[1][veg_dindx0                ] &&
																valid_vegetation[1][veg_dindx0 +             1] &&
																valid_vegetation[1][veg_dindx0 + woi.width    ] &&
																valid_vegetation[1][veg_dindx0 + woi.width + 1])) {
													auses_veg[windx].getAndIncrement();
													aused_veg[veg_dindx0                ].getAndIncrement();
													aused_veg[veg_dindx0 +             1].getAndIncrement();
													aused_veg[veg_dindx0 + woi.width    ].getAndIncrement();
													aused_veg[veg_dindx0 + woi.width + 1].getAndIncrement();
												}
											}
										}
									}
								}
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		int [][] num_uses = new int [2][auses_veg.length];
		for (int i = 0; i < num_uses[0].length; i++) {
			num_uses[0][i] = auses_veg[i].get();
			num_uses[1][i] = aused_veg[i].get();
		}
		return num_uses;
	}
	
//	public boolean [] getValidVegetation
	
	
	
}
