/**
 **
 ** OrthoEqualizeLMA - Fit multiple scenes pixel values (temperature)
 **                linear transformations using already calculated pairwise
 **                transformations
 **
 ** Copyright (C) 2024 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  OrthoEqualizeLMA.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.orthomosaic;

import java.awt.Point;
import java.io.IOException;
import java.time.temporal.ChronoUnit;
import java.time.temporal.TemporalUnit;
import java.util.ArrayList;
import java.util.concurrent.atomic.AtomicInteger;

import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.common.GenericJTabbedDialog;
import com.elphel.imagej.tileprocessor.ImageDtt;

import Jama.Matrix;

public class OrthoEqualizeLMA {
	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 []         parameters_vector = null;
	private double []         y_vector =        null;
	private double            pure_weight; // weight of samples only as a fraction of total weight
	private double []         weights; // normalized so sum is 1.0 for all - samples and extra regularization
	private double []         last_ymfx =       null;
	private double [][]       last_jt =         null;
	private PairwiseOrthoMatch [][] matches =   null;
	private int [][]          pairs =           null;
	private int [][]          pairs_ni =        null; // non-intersecting pairs for multi-threaded processing
	private int []            indices =         null;
	private int               num_scenes =      0;
	private int               num_pairs =       0;
	
	public static int buildEqualize(
			CLTParameters       clt_parameters,
			OrthoMapsCollection maps_collection,
			String              orthoMapsCollection_path) {
		boolean ignore_equalize=  clt_parameters.imp.pequ_ignore_equalize; // false; // ignore previous equalization
        boolean use_inv =         clt_parameters.imp.pequ_use_inv;         // false;
		double  scale_weight =    clt_parameters.imp.pequ_scale_weight;    // 500;     // relative weight of scale differences compared to offset differences
		double  pull_weight =     clt_parameters.imp.pequ_pull_weight;     // 0.001; // relative weight of offsets and scales differences from 1.0 to pairs mismatch
		double  half_weight_sec = clt_parameters.imp.pequ_half_weight_sec; // 300.0;   //  - time difference to reduce weight twice
		double  min_weight_sec =  clt_parameters.imp.pequ_min_weight_sec;  // 0.01;  //  weight of pairs at very different time
        double  overlap_pow =     clt_parameters.imp.pequ_overlap_pow;     // 2.0; // match weight as overlap fraction to this power
		double  rms_diff =        clt_parameters.imp.pequ_rms_diff;        // 0.000001;
		int     num_iter =        clt_parameters.imp.pequ_num_iter;        // 100;    //  50;
		int     debugLevel =      clt_parameters.imp.pequ_debugLevel;      // 2;
		
		GenericJTabbedDialog gd = new GenericJTabbedDialog("Pairwise Match Parameters",1200,400);        
		gd.addCheckbox    ("Ignore existing equalization",ignore_equalize, "Ignore existing equalization, start from scratch (a=1, b=0).");
		gd.addCheckbox    ("Use reversed pairs",          use_inv,     "Use reversed (late-early timestamps) pairs.");
		gd.addNumericField("Relative scale weight",       scale_weight,  3,7,"",	"Importance of a-coefficient (scale) relative to b-coefficient (offset).");
		gd.addNumericField("Pull weight",                 pull_weight,  3,7,"",	"Relative weight of offsets and scales differences from 1.0 to pairs mismatch.");
		gd.addNumericField("Half-weight time difference", half_weight_sec,  3,7,"s",	"Time difference (in a pair) to reduce weight twice.");
		gd.addNumericField("Weight for large time offset",min_weight_sec,  3,7,"",	"Weight of pairs at very different time.");
		gd.addNumericField("Overlap inportance",          overlap_pow,  3,7,"",	"Raise overlap fraction (of the smaller image) to this power before using as weight.");
		gd.addNumericField("RMSE relative improvement",   rms_diff,     8,10,"",	"Relative RMSE improvement to exit LMA.");
		gd.addNumericField("LMA iterations",              num_iter, 0,3,"",".Maximal number of the LMA iterations.");
		gd.addNumericField("Debug level for equalization",debugLevel, 0,3,"","Debug level for global (LMA-based) intensity equalization.");
 		gd.showDialog();
		if (gd.wasCanceled()) return -1;
        
        ignore_equalize =           gd.getNextBoolean();
        use_inv =                   gd.getNextBoolean();
        scale_weight =              gd.getNextNumber();
        pull_weight =               gd.getNextNumber();
        half_weight_sec =           gd.getNextNumber();
        min_weight_sec =            gd.getNextNumber();
        overlap_pow =               gd.getNextNumber();
        rms_diff =                  gd.getNextNumber();
        num_iter =            (int) gd.getNextNumber();
        debugLevel =          (int) gd.getNextNumber();
		
		return buildEqualize(
				clt_parameters,           // CLTParameters       clt_parameters,
				maps_collection,          // OrthoMapsCollection maps_collection,
				orthoMapsCollection_path, // String              orthoMapsCollection_path,
				ignore_equalize,          // boolean ignore_equalize,
				use_inv,                  // boolean use_inv,
				scale_weight,             // double  scale_weight,
				pull_weight,              // double  pull_weight,
				half_weight_sec,          // double  half_weight_sec,
				min_weight_sec,           // double  min_weight_sec,
				overlap_pow,              // double  overlap_pow,
				rms_diff,                 // double  rms_diff,
				num_iter,                 // int     num_iter,
				debugLevel);              // int     debugLevel);
	}
		
		
	public static int buildEqualize(
			CLTParameters       clt_parameters,
			OrthoMapsCollection maps_collection,
			String              orthoMapsCollection_path,
			boolean ignore_equalize,
			boolean use_inv,
			double  scale_weight,
			double  pull_weight,
			double  half_weight_sec,
			double  min_weight_sec,
			double  overlap_pow,
			double  rms_diff,
			int     num_iter,
			int     debugLevel) {

		double  lambda =            0.1;
		double  lambda_scale_good = 0.5;
		double  lambda_scale_bad =  8.0;
		double  lambda_max =      100;
		boolean last_run =       false;


		int [] indices = maps_collection.getScenesSelection(
				clt_parameters, // CLTParameters       clt_parameters,				
				" to build a map"); // String purpose)
		OrthoEqualizeLMA oel = new OrthoEqualizeLMA();
		oel.prepareLMA (
				clt_parameters, // CLTParameters       clt_parameters,
				maps_collection, // OrthoMapsCollection maps_collection,
				indices,         // int []              indices,
				ignore_equalize, // boolean             ignore_equalization, // ignore previous equalization
				// scale_weights applies both to pairs mismatch and per-scene values difference from neutral {1.0, 0.0}
				scale_weight,    // double              scale_weight,    // relative weight of scale differences compared to offset differences  
				pull_weight,     // double              pull_weight,     // relative weight of offsets and scales differences from 1.0 to pairs mismatch
				half_weight_sec, // double              half_weight_sec, // 300 - time difference to reduce weight twice
				min_weight_sec,  // double              min_weight_sec,  // 0.01 weight of pairs at very different time
				use_inv,         // boolean             use_inv,         // use inverse pairs
				overlap_pow,     // double              overlap_pow, // match weight as overlap fraction to this power
				debugLevel);     // int                 debugLevel);
		int lma_rslt = oel.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
				lambda,             // double lambda,           // 0.1
				lambda_scale_good,  // double lambda_scale_good,// 0.5
				lambda_scale_bad,   // double lambda_scale_bad, // 8.0
				lambda_max,         // double lambda_max,       // 100
				rms_diff,           // double rms_diff,         // 0.001
				num_iter,           // int    num_iter,         // 20
				last_run,           // boolean last_run,
				null, // dbg_lma_prefix,     // String dbg_prefix,
				debugLevel);        // int    debug_level)
		System.out.println("lma_rslt="+lma_rslt); 
		double [] rms = oel.getRms();
		double [] initial_rms = oel.getInitialRms();
		if (debugLevel > 0) {
			System.out.println("LMA: full RMS="+rms[0]+" ("+initial_rms[0]+"), pure RMS="+rms[1]+" ("+initial_rms[1]+")");
		}
        //Get and apply affines
		double [][] equalize = oel.getEqualize();
		double [] fx = oel.getFx();
    	oel.updateEqualize(maps_collection);
        if (orthoMapsCollection_path != null) {
        	try {
        		maps_collection.writeOrthoMapsCollection(orthoMapsCollection_path);
        	} catch (IOException e) {
        		// TODO Auto-generated catch block
        		e.printStackTrace();
        	}
        	System.out.println("Saved data to "+ orthoMapsCollection_path);
        }
		return 0;
	}	
	
	public double [] getParameters() {
		return parameters_vector;
	}
	
	public double [] getFx() {
		return getFxDerivs(
				parameters_vector, // double []         vector,
				null,              // final double [][] jt, // should be null or initialized with [vector.length][]
				0);               // final int         debug_level)
	}

	public int prepareLMA (
			CLTParameters       clt_parameters,
			OrthoMapsCollection maps_collection,
			int []              indices,
			// scale_weights applies both to pairs mismatch and per-scene values difference from neutral {1.0, 0.0}
			boolean             ignore_equalization, // ignore previous equalization
	        double              scale_weight,    // relative weight of scale differences compared to offset differences  
	        double              pull_weight,     // relative weight of offsets and scales differences from 1.0 to pairs mismatch
	        double              half_weight_sec, // 300 - time difference to reduce weight twice
	        double              min_weight_sec,  // 0.01 weight of pairs at very different time
            boolean             use_inv,         // use inverse pairs
            double              overlap_pow, // match weight as overlap fraction to this power
			int                 debugLevel) {
		this.indices = indices;
		num_scenes = indices.length;
		matches = new PairwiseOrthoMatch[indices.length][indices.length];
		ArrayList<Point> pairs_list = new ArrayList<Point>();
		for (int i = 0; i < num_scenes-1; i++) {
			int scene0 = indices[i]; 
			for (int j = i+1; j < num_scenes; j++){
				int scene1 = indices[j];
				PairwiseOrthoMatch match = maps_collection.ortho_maps[scene0].getMatch(maps_collection.ortho_maps[scene1].getName());
				PairwiseOrthoMatch inv_match = use_inv?
						maps_collection.ortho_maps[scene1].getMatch(maps_collection.ortho_maps[scene0].getName()):null;
				if ((match != null) || (inv_match != null)){
					if (match == null) {
						double [] enuOffset = maps_collection.ortho_maps[scene0].enuOffsetTo(maps_collection.ortho_maps[scene1]);
						double [] rd = {enuOffset[0], -enuOffset[1]}; // {right,down} of the image 
						match = inv_match.getInverse(rd);
					}
					if (inv_match == null) {
						double [] enuOffset = maps_collection.ortho_maps[scene1].enuOffsetTo(maps_collection.ortho_maps[scene0]);
						double [] rd = {enuOffset[0], -enuOffset[1]}; // {right,down} of the image 
						inv_match = match.getInverse(rd);
					}
				}
				if ((match != null) && match.isSetEqualize2to1()) {
					matches[i][j] = match;
					matches[j][i] = inv_match;
					pairs_list.add(new Point(i,j)); // only once?
				}
			}
		}
		num_pairs = pairs_list.size();
		pairs =   new int[num_pairs][2];
		for (int np = 0; np < num_pairs; np++) {
			Point pair = pairs_list.get(np);
			pairs[np] = new int[] {pair.x, pair.y};
		}
		pairs_ni = OrthoMultiLMA.createNonIntersectingPairs(pairs);
		y_vector =          new double [2*num_pairs + 2*num_scenes];
		weights =           new double [2*num_pairs + 2*num_scenes];
		parameters_vector = new double [2*num_scenes];
		int sec_24hrs = 60*60*24;
		double sw = 0;
		// pairwise matches
		for (int npair = 0; npair < num_pairs; npair++) {
			Point p = pairs_list.get(npair);
			PairwiseOrthoMatch match = matches[p.x][p.y];
			y_vector[2*npair + 0] = match.equalize1to0[0];
			y_vector[2*npair + 1] = match.equalize1to0[1];
			double diff_seconds =  Math.abs((int) ChronoUnit.SECONDS.between(
					maps_collection.ortho_maps[indices[p.x]].getLocalDateTime(),
					maps_collection.ortho_maps[indices[p.y]].getLocalDateTime()
					)) % sec_24hrs;
			if (diff_seconds > (sec_24hrs/2)) {
				diff_seconds = sec_24hrs-diff_seconds;
			}
			// weight reduction because of different capture time
			double weight_dt = Math.max(Math.pow(0.5, diff_seconds/half_weight_sec),min_weight_sec);
			// weight reduction because of partial overlap
			double weight_overlap = Math.pow(match.getOverlap(), overlap_pow);
			// scale_weight
			double w = weight_dt * weight_overlap;
			weights[2*npair + 0] = w * scale_weight;
			weights[2*npair + 1] = w;
			sw += w *(1 + scale_weight);
		}
		double swp = sw;
		// individual scene pulls to {1.0,0} for regularization
		for (int nscene = 0; nscene < num_scenes; nscene++) {
			double [] equalize =  ignore_equalization?
					(new double[] {1.0,0.0}):
						maps_collection.ortho_maps[indices[nscene]].getEqualize();
			int np = 2 * nscene;
			int indx = 2 * num_pairs + np;
			parameters_vector[np + 0] = equalize[0];
			parameters_vector[np + 1] = equalize[1];
			y_vector[indx + 0] = 1.0;
			y_vector[indx + 1] = 0.0;
			weights [indx + 0] = scale_weight * pull_weight;
			weights [indx + 1] = pull_weight;
			sw+= pull_weight * (1 + scale_weight);
		}
		pure_weight = swp/sw;
		double s = 1.0/sw;
		for (int i = 0; i <weights.length;i++) {
			weights[i] *=s;
		}
		last_jt = new double [parameters_vector.length][];
		return weights.length;
	}
	
	/**
	 * Converting to use available_pairs[][] instead of indices[] - not yet used
	 * @param clt_parameters
	 * @param maps_collection
	 * @param available_pairs
	 * @param ignore_equalization
	 * @param scale_weight
	 * @param pull_weight
	 * @param half_weight_sec
	 * @param min_weight_sec
	 * @param use_inv
	 * @param overlap_pow
	 * @param debugLevel
	 * @return
	 */
	public int prepareLMA (
			CLTParameters       clt_parameters,
			OrthoMapsCollection maps_collection,
			int [][]            available_pairs,
			// scale_weights applies both to pairs mismatch and per-scene values difference from neutral {1.0, 0.0}
			boolean             ignore_equalization, // ignore previous equalization
	        double              scale_weight,    // relative weight of scale differences compared to offset differences  
	        double              pull_weight,     // relative weight of offsets and scales differences from 1.0 to pairs mismatch
	        double              half_weight_sec, // 300 - time difference to reduce weight twice
	        double              min_weight_sec,  // 0.01 weight of pairs at very different time
            boolean             use_inv,         // use inverse pairs
            double              overlap_pow, // match weight as overlap fraction to this power
			int                 debugLevel) {
		int [] indices = maps_collection.getScenesFromPairs( // may be shorter, each element - absolute scene number used in pairs
				available_pairs,  // pairs_defined_abs,// int [][] pairs,
				null);            // int [] indices_in)  // preselected indices or null
		int [][] defined_pairs =   maps_collection.condensePairs (available_pairs, indices); // pairs_defined_abs,indices);
		
		this.indices = indices;
		num_scenes = indices.length;
		matches = new PairwiseOrthoMatch[indices.length][indices.length];
		ArrayList<Point> pairs_list = new ArrayList<Point>();
		for (int i = 0; i < num_scenes-1; i++) {
			int scene0 = indices[i]; 
			for (int j = i+1; j < num_scenes; j++){
				int scene1 = indices[j];
				PairwiseOrthoMatch match = maps_collection.ortho_maps[scene0].getMatch(maps_collection.ortho_maps[scene1].getName());
				PairwiseOrthoMatch inv_match = use_inv?
						maps_collection.ortho_maps[scene1].getMatch(maps_collection.ortho_maps[scene0].getName()):null;
				if ((match != null) || (inv_match != null)){
					if (match == null) {
						double [] enuOffset = maps_collection.ortho_maps[scene0].enuOffsetTo(maps_collection.ortho_maps[scene1]);
						double [] rd = {enuOffset[0], -enuOffset[1]}; // {right,down} of the image 
						match = inv_match.getInverse(rd);
					}
					if (inv_match == null) {
						double [] enuOffset = maps_collection.ortho_maps[scene1].enuOffsetTo(maps_collection.ortho_maps[scene0]);
						double [] rd = {enuOffset[0], -enuOffset[1]}; // {right,down} of the image 
						inv_match = match.getInverse(rd);
					}
				}
				if ((match != null) && match.isSetEqualize2to1()) {
					matches[i][j] = match;
					matches[j][i] = inv_match;
					pairs_list.add(new Point(i,j)); // only once?
				}
			}
		}
		num_pairs = pairs_list.size();
		pairs =   new int[num_pairs][2];
		for (int np = 0; np < num_pairs; np++) {
			Point pair = pairs_list.get(np);
			pairs[np] = new int[] {pair.x, pair.y};
		}
		pairs_ni = OrthoMultiLMA.createNonIntersectingPairs(pairs);
		y_vector =          new double [2*num_pairs + 2*num_scenes];
		weights =           new double [2*num_pairs + 2*num_scenes];
		parameters_vector = new double [2*num_scenes];
		int sec_24hrs = 60*60*24;
		double sw = 0;
		// pairwise matches
		for (int npair = 0; npair < num_pairs; npair++) {
			Point p = pairs_list.get(npair);
			PairwiseOrthoMatch match = matches[p.x][p.y];
			y_vector[2*npair + 0] = match.equalize1to0[0];
			y_vector[2*npair + 1] = match.equalize1to0[1];
			double diff_seconds =  Math.abs((int) ChronoUnit.SECONDS.between(
					maps_collection.ortho_maps[indices[p.x]].getLocalDateTime(),
					maps_collection.ortho_maps[indices[p.y]].getLocalDateTime()
					)) % sec_24hrs;
			if (diff_seconds > (sec_24hrs/2)) {
				diff_seconds = sec_24hrs-diff_seconds;
			}
			// weight reduction because of different capture time
			double weight_dt = Math.max(Math.pow(0.5, diff_seconds/half_weight_sec),min_weight_sec);
			// weight reduction because of partial overlap
			double weight_overlap = Math.pow(match.getOverlap(), overlap_pow);
			// scale_weight
			double w = weight_dt * weight_overlap;
			weights[2*npair + 0] = w * scale_weight;
			weights[2*npair + 1] = w;
			sw += w *(1 + scale_weight);
		}
		double swp = sw;
		// individual scene pulls to {1.0,0} for regularization
		for (int nscene = 0; nscene < num_scenes; nscene++) {
			double [] equalize =  ignore_equalization?
					(new double[] {1.0,0.0}):
						maps_collection.ortho_maps[indices[nscene]].getEqualize();
			int np = 2 * nscene;
			int indx = 2 * num_pairs + np;
			parameters_vector[np + 0] = equalize[0];
			parameters_vector[np + 1] = equalize[1];
			y_vector[indx + 0] = 1.0;
			y_vector[indx + 1] = 0.0;
			weights [indx + 0] = scale_weight * pull_weight;
			weights [indx + 1] = pull_weight;
			sw+= pull_weight * (1 + scale_weight);
		}
		pure_weight = swp/sw;
		double s = 1.0/sw;
		for (int i = 0; i <weights.length;i++) {
			weights[i] *=s;
		}
		last_jt = new double [parameters_vector.length][];
		return weights.length;
	}

	
	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 "+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 > 3) {
				/*
				 dbgJacobians(
							corr_vector, // GeometryCorrection.CorrVector corr_vector,
							1E-5, // double delta,
							true); //boolean graphic)
				*/
			}
		}
		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)
		
		if (debug_level>2) {
			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>2) {
			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 > 2) {
				// 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;
	}
	
	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 double []     wymfw =       new double [fx.length];
		double [] swd2 = new double[num_pairs];
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nPair = ai.getAndIncrement(); nPair < num_pairs; nPair = ai.getAndIncrement()) {
						int np2 = 2*nPair;
						for (int i1 = 0; i1 < 2; i1++) {
							int i = np2+i1;
							double d = y_vector[i] - fx[i];
							double wd = d * weights[i];
							wymfw[i] = wd;
							swd2[nPair] += d * wd;
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		double s_rms = 0;
		for (int nPair = 0; nPair < num_pairs; nPair++) {
			s_rms += swd2[nPair];
		}
		double s_rms_pure = s_rms/pure_weight; 
		// num_scenes is not that large; sum w/o threads
		for (int nScene = 0; nScene < num_scenes; nScene++) {
			for (int i1 = 0; i1 < 2; i1++) {
				int i = 2*num_pairs + 2*nScene + i1;
				double d = y_vector[i] - fx[i];
				double wd = d * weights[i];
				wymfw[i] = wd;
				s_rms +=  d * wd;
			}
		}
		if (rms_fp != null) {
			rms_fp[0] = Math.sqrt(s_rms);
			if (rms_fp.length > 1) {
				rms_fp[1] = Math.sqrt(s_rms_pure);
			}
		}
		return wymfw;
	}
	
	public double [] getRms() {
		return last_rms;
		
	}

	public double [] getInitialRms() {
		return initial_rms;
	}	
	
	
	public void updateEqualize(OrthoMapsCollection maps_collection) {
		for (int nscene = 0; nscene < num_scenes; nscene++) {
			double [] equalize =  maps_collection.ortho_maps[indices[nscene]].getEqualize();
			equalize[0] = parameters_vector[2*nscene + 0];
			equalize[1] = parameters_vector[2*nscene + 1];
		}
	}
	
	public double [][] getEqualize(){
		double [][] equalize = new double [num_scenes][];
		for (int nscene = 0; nscene < num_scenes; nscene++) {
			equalize[nscene] = new double [] {parameters_vector[2*nscene+0],parameters_vector[2*nscene+1]};
		}
		return equalize;
	}
	//equalize	
	
	
	private double [] getFxDerivs(
			final double []   vector,
			final double [][] jt, // should be null or initialized with [vector.length][]
			final int         debug_level)
	{
		final double [] fx = new double [weights.length]; // weights.length]; : weights.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 pair_group = 0; pair_group<pairs_ni.length; pair_group++) {
			final int fpair_group = pair_group;
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					public void run() {
						for (int iPair = ai.getAndIncrement(); iPair < pairs_ni[fpair_group].length; iPair = ai.getAndIncrement())  {
							int nPair = pairs_ni[fpair_group][iPair]; // index in pairs[][]
							int [] pair = pairs[nPair];
							double a0= vector[2*pair[0]+0], a1=vector[2*pair[1]+0];
							double b0= vector[2*pair[0]+1], b1=vector[2*pair[1]+1];
							double a = a1/a0;
							double b = (b1-b0)/a0;
							fx[2*nPair+0] = a;
							fx[2*nPair+1] = b;
							if (jt != null) {
								jt[2*pair[0] + 0][2*nPair + 0] += -a1/a0/a0;      // da/da0
								jt[2*pair[0] + 0][2*nPair + 1] += -(b1-b0)/a0/a0; // db/da0
								jt[2*pair[1] + 0][2*nPair + 0] +=  1/a0;          // da/da1
///								jt[2*pair[1] + 0][2*nPair + 1] +=  0;             // db/da1
///								jt[2*pair[0] + 1][2*nPair + 0] +=  0;             // da/db0
								jt[2*pair[0] + 1][2*nPair + 1] += -1/a0;          // db/db0
///								jt[2*pair[1] + 1][2*nPair + 0] +=  0;             // da/db1
								jt[2*pair[1] + 1][2*nPair + 1] +=  1/a0;          // db/db1
							}
						}
					}
				};
			}		      
			ImageDtt.startAndJoin(threads);
			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 npar = 2 * nScene;
						int ny =   2 * num_pairs + 2 * nScene;
						double a = vector[npar + 0];
						double b = vector[npar + 1];
						fx[ny + 0] = a;
						fx[ny + 1] = b/a;
						if (jt != null) {
							jt[npar + 0][ny + 0] =  1;
							jt[npar + 0][ny + 1] =  -b/a/a;
///							jt[npar + 1][ny + 0] =  0;
							jt[npar + 1][ny + 1] =  1/a;
						}						
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return fx;
	}
}
