/**
 **
 ** OrthoMultiLMA - Fit multiple scenes orthographic using pair-wise affine
 **                transform matrices already calculated.
 **
 ** Copyright (C) 2024 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  OrthoMultiLMA.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.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.Comparator;
import java.util.HashMap;
import java.util.Set;
import java.util.concurrent.atomic.AtomicInteger;

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

import Jama.Matrix;
import ij.ImagePlus;
import ij.gui.PointRoi;

public class OrthoMultiLMA {
	final   boolean           move_only;
	final   boolean           corr_avg; // correct average skew, tilt, scale
	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 []         x_vector =        null; // not used. Save total weight
	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            pure_weight; // weight of samples only	
	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 double [][]       offsets =         null; // scene offsets (rd)
	private int               num_scenes =      0;
	private int               num_pairs =       0;
	public OrthoMultiLMA (
			boolean corr_avg,
			boolean move_only
			) {
		this.corr_avg = corr_avg && !move_only;
		this.move_only = move_only;
	}
	public static boolean testMultiLMA(
			CLTParameters       clt_parameters,
			OrthoMapsCollection maps_collection,
			int []              indices) {
		boolean       move_only = false;
		int scene0 = indices[0], scene1 = indices[1];
		PairwiseOrthoMatch direct = maps_collection.ortho_maps[scene0].getMatch(maps_collection.ortho_maps[scene1].getName());
		double [] enuOffset = maps_collection.ortho_maps[scene1].enuOffsetTo(maps_collection.ortho_maps[scene0]);
		double [] rd = {enuOffset[0], -enuOffset[1]}; // {right,down} of the image 
		PairwiseOrthoMatch inv_match = direct.getInverse(rd);
		double [][] affine0 = new double [2][3]; // inv_match.affine()
		double [][] affine1 = new double [2][3]; // inv_match.affine()
		affine0[0][0] = 0.5 * inv_match.getAffine()[0][0];
		double [][] E = {{1,0,0},{0,1,0}};
		for (int i = 0; i < 2; i++) {
			for (int j = 0; j < 3; j++) {
				affine0[i][j] = 0.5 * (inv_match.getAffine()[i][j]+E[i][j]);
				affine1[i][j] = 0.5 * (direct.getAffine()[i][j]+E[i][j]);
			}
		}
		
		
		
		String [] titles = new String [indices.length];
		for (int i = 0; i < indices.length; i++) {
			titles[i] = indices[i]+"-"+maps_collection.ortho_maps[indices[i]].getName();
		}
		double [][][] affines_0d = {E,direct.getAffine()};
		double [][][] affines_sym = {affine0, affine1};
		
		double [][] affine0a = new double [2][3]; // keep b[] ==0
		for (int i = 0; i < 2; i++) {
			for (int j = 0; j < 2; j++) {
				affine0a[i][j] = 0.5* affine0[i][j]; 
			}
		}
		
		double [][] affine1a = PairwiseOrthoMatch.combineAffines( 
				affine0a,           // double [][] affine0,
				direct.getAffine(), //double [][] affine, // differential
				rd);                // double [] rd);
		
		PairwiseOrthoMatch test_match = new PairwiseOrthoMatch (
				affine0a, // double [][] affine0,
				affine1a, // double [][] affine1,
				rd); // double [] rd);
		
		double [][][] affines_a = {affine0a,affine1a};
		// show as intended (E, diff)
        int         zoom_level = -2;
        int [] wh = new int[2];
        int [] origin = new int[2];
        double [][] centers = new double [indices.length][];
		double [][] dmulti = maps_collection.renderMultiDouble (
				null,       // double [][]   ground_planes, // null - images, non-null altitudes. use new double[2][3] for old way alt
				indices,    // int []        indices, // null or which indices to use (normally just 2 for pairwise comparison)
				true,       // boolean       bounds_to_indices,
				affines_a, // affines_0d, // affines,    // double [][][] affines, // null or [indices.length][2][3]
				null,              // double [][]   equalize,
				true,              // boolean       ignore_equalize,
				null,       // warp,       // FineXYCorr    warp,,
				zoom_level, // int         zoom_level,
				wh,         // int []      wh,
				origin,     // int []      origin){ // maps[0] as a reference
				centers);   // double [][] centers)
		
		ImagePlus imp_multi = ShowDoubleFloatArrays.makeArrays(
				dmulti,
				wh[0],
				wh[1],
				"scaled.tiff", //"direct_diff.tiff",				
				titles); // test_titles,
    	PointRoi roi = new PointRoi();
    	for (int i = 0; i < centers.length; i++) {
    		roi.addPoint(centers[i][0],centers[i][1], 1+i);
    	}
    	roi.setOptions("label");
    	imp_multi.setRoi(roi);
		imp_multi.show();
        // show half_offsets
		double [][] dmulti_sym = maps_collection.renderMultiDouble (
				null,       // double [][]   ground_planes, // null - images, non-null altitudes. use new double[2][3] for old way alt
				indices,    // int []        indices, // null or which indices to use (normally just 2 for pairwise comparison)
				true,       // boolean       bounds_to_indices,
				affines_sym,// affines,    // double [][][] affines, // null or [indices.length][2][3]
				null,              // double [][]   equalize,
				true,              // boolean       ignore_equalize,
				null,       // warp,       // FineXYCorr    warp,,
				zoom_level, // int         zoom_level,
				wh,         // int []      wh,
				origin,     // int []      origin){ // maps[0] as a reference
				centers);   // double [][] centers)
		
		ImagePlus imp_sym = ShowDoubleFloatArrays.makeArrays(
				dmulti_sym,
				wh[0],
				wh[1],
				"direct_sym.tiff",				
				titles); // test_titles,
    	PointRoi roi_sym = new PointRoi();
    	for (int i = 0; i < centers.length; i++) {
    		roi_sym.addPoint(centers[i][0],centers[i][1], 1+i);
    	}
    	roi_sym.setOptions("label");
    	imp_sym.setRoi(roi_sym);
    	imp_sym.show();
		
		
		
		
		
		
		
		
		
		
		
		double [][][] derivs =       new double [12][][];
		double [][][] derivs_delta = new double [12][][];
		double delta = 1E-7;
		getAffineAndDerivetives_delta(
				move_only, //boolean       move_only,
				delta,   // double        delta,
				affine0, // double [][]   affine00,
				affine1, // double [][]   affine10,
				rd,      // double []     rd,
				derivs_delta); // double [][][] derivs)
		double [][] affine = 		getAffineAndDerivatives(
				move_only, //boolean       move_only,
				affine0, // double [][]   affine00,
				affine1, // double [][]   affine10,
				rd,      // double []     rd,
				derivs); // double [][][] derivs)
		double [][][] err = new double [12][2][3];
		for (int n = 0; n < err.length; n++) if ((derivs[n] != null) && (derivs_delta[n] != null)) {
			for (int i = 0; i < err[n].length; i++) {
				for (int j = 0; j < err[n][i].length; j++) {
					err[n][i][j] = derivs[n][i][j] - derivs_delta[n][i][j];
				}
			}			
		}
		System.out.println();
		return true;
	}
	
	public static int buildOrthoMap(
			CLTParameters       clt_parameters,
			OrthoMapsCollection maps_collection,
			String              orthoMapsCollection_path
			) {
		ArrayList<Point> pairs_list = new ArrayList<Point>(); 
		for (OrthoMap map : maps_collection.ortho_maps) {
			for (String other_name: map.pairwise_matches.keySet()) {
				pairs_list.add(new Point(
						maps_collection.getIndex(map.getName()),
						maps_collection.getIndex(other_name)));
			}
		}
		// sort pairs_list by x then y
		Collections.sort(pairs_list, new Comparator<Point>() {
			@Override
			public int compare(Point lhs, Point rhs) {
				return (rhs.x > lhs.x) ? -1 : (rhs.x < lhs.x) ? 1 :
					((rhs.y > lhs.y) ? -1 : (rhs.y < lhs.y) ? 1 : 0); // increasing
			}
		});

		// convert ArrayList<Point> to array int[][]
		int [][] available_pairs = new int [pairs_list.size()][2];
		for (int i = 0; i < available_pairs.length; i++) {
			available_pairs[i][0] = pairs_list.get(i).x;
			available_pairs[i][1] = pairs_list.get(i).y;
		}

		
		
		
		
		
///		boolean all_pairs =      true;
///		double  min_overlap_frac = clt_parameters.imp.pwise_overlap; // 0.25;
///		double  max_rms_refine =      clt_parameters.imp.pwise_max_rms;  // 0.35; //
		
		boolean flt_list =           clt_parameters.imp.flt_list;          //  true;
		boolean flt_undef_only =     clt_parameters.imp.flt_undef_only;    //  false;
		double  flt_min_overlap =    clt_parameters.imp.flt_min_overlap;   //  0.0;
		double  flt_max_overlap =    clt_parameters.imp.flt_max_overlap;   //  1.0;
		double  flt_min_rms =        clt_parameters.imp.flt_min_rms;       //  0.0;
		double  flt_max_rms =        clt_parameters.imp.flt_max_rms;       //  2.0;
		boolean flt_nan_rms =        false; // clt_parameters.imp.flt_nan_rms;       //  false;
		boolean flt_filt_zoom =      clt_parameters.imp.flt_filt_zoom;     //  true;
		int     flt_min_zoom =       clt_parameters.imp.flt_min_zoom;      // -2;
		int     flt_max_zoom =       clt_parameters.imp.flt_max_zoom;      // 10;
		double  flt_min_sfm =        clt_parameters.imp.flt_min_sfm;       //  0.0;
		double  flt_max_sfm =        clt_parameters.imp.flt_max_sfm;       //1000.0;
        int     flt_alt =            clt_parameters.imp.flt_alt;           // 0;
		boolean flt_show_names =     true;  // clt_parameters.imp.flt_show_names;    //  true;
		boolean flt_show_overlaps =  true;  // clt_parameters.imp.flt_show_overlaps; //  true;
		boolean flt_show_rms =       true;  // clt_parameters.imp.flt_show_rms;      //  true;
		boolean flt_show_zoom =      true;  // clt_parameters.imp.flt_show_zoom;     //  true;
		boolean flt_show_alt =       true;  // clt_parameters.imp.flt_show_alt;     //  true;
		
		boolean move_only =      clt_parameters.imp.pmap_move_only;      // false;
		boolean ignore_affines = clt_parameters.imp.pmap_ignore_affines; // false;
        boolean use_inv =        clt_parameters.imp.pmap_use_inv;        // false;
        double  skew_pull =      clt_parameters.imp.pmap_skew_pull;      // 1.0;
        double  tilt_pull =      clt_parameters.imp.pmap_tilt_pull;      // 1.0;
        double  scale_pull =     clt_parameters.imp.pmap_scale_pull;     // 0.1; // .0;
		double  position_pull =  clt_parameters.imp.pmap_position_pull;  // 0.0001;
        double  overlap_pow =    clt_parameters.imp.pmap_overlap_pow;    // 2.0; // match weight as overlap fraction to this power
		double  rms_diff =       clt_parameters.imp.pmap_rms_diff;       // 0.000001;
		int     num_iter =       clt_parameters.imp.pmap_num_iter;       // 100; //  50;
		boolean show_result =    clt_parameters.imp.pmap_show_result;    // false;
		int     debugLevel =     clt_parameters.imp.pmap_debugLevel;     // 2;
		boolean flt_update_config = false;

		GenericJTabbedDialog gd = new GenericJTabbedDialog("Pairwise Match Parameters",1200,800);
///		gd.addCheckbox    ("Use all available pairs",  all_pairs,  "Use all available pairs.");
///		gd.addNumericField("Minimal overlap fraction",    min_overlap_frac,  3,7,"",	    "Minimal overlap fraction.");
///		gd.addNumericField("Satisfactory RMSE, final",    max_rms_refine,3,7,"scaled pix",	"Maximal RMSE to consider match, in scaled pixels, during refine.");
		gd.addCheckbox    ("Filter pairs",                flt_list, "Filter available pairs.");
		gd.addCheckbox    ("Keep undefined pairs only",   flt_undef_only, "Keep only undefined pairs (affines== null).");
		gd.addNumericField("Minimal scene overlap (0..1)",flt_min_overlap,  3,7,"", "Minimal overlap of the scenes to keep (0-no overlap, 1.0 - smaller scene is inside the parger one.");
		gd.addNumericField("Maximal scene overlap (0..1)",flt_max_overlap,  3,7,"", "Maximal overlap of the scenes to keep (0-no overlap, 1.0 - smaller scene is inside the parger one.");
		gd.addNumericField("Minimal RMSE",                flt_min_rms,  3,7,"",	 "Minimal LMA RMSE of the scene pair.");
		gd.addNumericField("Maximal RMSE",                flt_max_rms,  3,7,"",	 "Maximal LMA RMSE of the scene pair.");
///		gd.addCheckbox    ("NaN RMS (failed match)",      flt_nan_rms, "Keep only failed matches with RMSE=NaN.");
		gd.addCheckbox    ("Filter by zoom level"  ,      flt_filt_zoom, "Filter by the zoom level used for matching.");
		gd.addNumericField("Minimal zoom",                flt_min_zoom, 0,3,"","Minimal zoom level used for matching.");
		gd.addNumericField("Maximal zoom",                flt_max_zoom, 0,3,"","Maximal zoom level used for matching.");
		gd.addNumericField("Minimal SfM gain",            flt_min_sfm,  3,7,"","Minimal SfM gain of the minimum in the scene pair.");
		gd.addNumericField("Maximal SfM gain",            flt_max_sfm,  3,7,"","Maximal SfM gain of the minimum in the scene pair.");
		gd.addChoice("Filter by pairwise ALT availability",IntersceneMatchParameters.FLT_ALT_MODES, IntersceneMatchParameters.FLT_ALT_MODES[flt_alt],"Filter by pairwise ALT availability.");
		gd.addCheckbox    ("Moves only",               move_only,  "Moves only, no affine transform.");
		gd.addCheckbox    ("Ignore existing affines",  ignore_affines, "Start from unity matrices, ignore saved affines.");
		gd.addCheckbox    ("Use reversed pairs",       use_inv,     "Use reversed (late-early timestamps) pairs.");
		gd.addNumericField("Skew pull",                skew_pull,    7,11,"",	"Intention to have axes perpendicular.");
		gd.addNumericField("Tilt pull",                tilt_pull,    7,11,"",	"Intention to have axes same length.");
		gd.addNumericField("Scale pull",               scale_pull,   7,11,"",	"Intention to have average scale of images = 1.0.");
		gd.addNumericField("Position pull",            position_pull,7,11,"",	"Intension to match IMS (GNSS) coordinates.");
		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,11,"",	"Relative RMSE improvement to exit LMA.");
		gd.addNumericField("LMA iterations",           num_iter, 0,3,"",".Maximal number of the LMA iterations.");
		gd.addCheckbox    ("Show result image",        show_result, ".");
		gd.addNumericField("Debug level for maps",     debugLevel, 0,3,"","Debug building for LMA building maps.");
		gd.addCheckbox    ("Update configuration",        flt_update_config, "Update matching configuration parameters to be saved as defaults.");

 		gd.showDialog();
		if (gd.wasCanceled()) return -1;
///		all_pairs =           gd.getNextBoolean();
///		min_overlap_frac =    gd.getNextNumber(); 
///		max_rms_refine   =    gd.getNextNumber();
		
		flt_list            = gd.getNextBoolean();
		flt_undef_only      = gd.getNextBoolean();
		flt_min_overlap     = gd.getNextNumber();
		flt_max_overlap     = gd.getNextNumber();
		flt_min_rms         = gd.getNextNumber();
		flt_max_rms         = gd.getNextNumber();
///		flt_nan_rms         = gd.getNextBoolean();
        flt_filt_zoom       = gd.getNextBoolean();
        flt_min_zoom =  (int) gd.getNextNumber(); 
        flt_max_zoom =  (int) gd.getNextNumber();
        flt_min_sfm         = gd.getNextNumber();
        flt_max_sfm         = gd.getNextNumber();
        flt_alt             = gd.getNextChoiceIndex();
        move_only =		      gd.getNextBoolean();
        ignore_affines =      gd.getNextBoolean();
        use_inv =             gd.getNextBoolean();
        skew_pull =           gd.getNextNumber();
        tilt_pull =           gd.getNextNumber();
        scale_pull =          gd.getNextNumber();
        position_pull =       gd.getNextNumber();
        overlap_pow =         gd.getNextNumber();
        rms_diff =            gd.getNextNumber();
        num_iter =      (int) gd.getNextNumber();
        show_result =         gd.getNextBoolean();
        debugLevel =    (int) gd.getNextNumber();
		flt_update_config =   gd.getNextBoolean();
		if (flt_update_config) {
			clt_parameters.imp.flt_list =            flt_list;
			clt_parameters.imp.flt_undef_only =      flt_undef_only;
			clt_parameters.imp.flt_min_overlap =     flt_min_overlap;
			clt_parameters.imp.flt_max_overlap =     flt_max_overlap;
			clt_parameters.imp.flt_min_rms =         flt_min_rms;
			clt_parameters.imp.flt_max_rms =         flt_max_rms;
///			clt_parameters.imp.flt_nan_rms =         flt_nan_rms;
			clt_parameters.imp.flt_filt_zoom =       flt_filt_zoom;
			clt_parameters.imp.flt_min_zoom =        flt_min_zoom;
			clt_parameters.imp.flt_max_zoom =        flt_max_zoom;
    		clt_parameters.imp.flt_min_sfm =         flt_min_sfm;
    		clt_parameters.imp.flt_max_sfm =         flt_max_sfm;
			clt_parameters.imp.flt_alt =             flt_alt;			
			clt_parameters.imp.pmap_move_only =      move_only;
			clt_parameters.imp.pmap_ignore_affines = ignore_affines;
	        clt_parameters.imp.pmap_use_inv =        use_inv;
	        clt_parameters.imp.pmap_skew_pull =      skew_pull;
	        clt_parameters.imp.pmap_tilt_pull =      tilt_pull;
	        clt_parameters.imp.pmap_scale_pull =     scale_pull;
			clt_parameters.imp.pmap_position_pull =  position_pull;
	        clt_parameters.imp.pmap_overlap_pow =    overlap_pow;
			clt_parameters.imp.pmap_rms_diff =       rms_diff;
			clt_parameters.imp.pmap_num_iter =       num_iter;
			clt_parameters.imp.pmap_show_result =    show_result;
			clt_parameters.imp.pmap_debugLevel =     debugLevel;
		}
	
		available_pairs = maps_collection.filterPairs(
				available_pairs, // int [][] plist_in,
				flt_undef_only,  // boolean  undef_only,
				flt_min_overlap, // double   min_overlap,
				flt_max_overlap, // double   max_overlap,
				flt_min_rms,     // double   min_rms,
				flt_max_rms,     // double   max_rms,
				flt_nan_rms,     // boolean  nan_rms)
				flt_filt_zoom,   // boolean  filt_zoom,
				flt_min_zoom,    // int      min_zoom,
				flt_max_zoom,    // int      max_zoom)
				flt_min_sfm,     // double   min_sfm,
				flt_max_sfm,     // double   max_sfm,
				flt_alt);        // int      flt_alt)

		String [] choices_all = maps_collection.textPairs (
				available_pairs,   // int [][] plist,
				flt_show_names,    // boolean          show_names,
				flt_show_overlaps, // boolean          show_overlap,
				flt_show_rms,      // boolean          show_rms,
				flt_show_zoom,     // boolean          show_zoom,
				flt_show_alt,      // boolean          show_alt,
				true,              // boolean          use_tab,
				null);             // String           extra_line)
		if (debugLevel > 0) {
			System.out.println("Selected "+available_pairs.length+" scene pairs for matching");
			for (int i = 0; i < available_pairs.length; i++) {
				System.out.println(String.format("%4d:%s",i,choices_all[i]));
			}
		}
		if (available_pairs.length == 0) {
			return -1;
		}
		
		return buildOrthoMap(
				clt_parameters,           // CLTParameters       clt_parameters,
				maps_collection,          // OrthoMapsCollection maps_collection,
				orthoMapsCollection_path, // String              orthoMapsCollection_path,
///				all_pairs,                // boolean             all_pairs,
				// filtering of the pairs
///				min_overlap_frac,         // double              min_overlap_frac,
///				max_rms_refine,           // double              max_rms,
				available_pairs,          // int [][]      available_pairs, 
				move_only,                // boolean             move_only,
				ignore_affines,           // boolean             ignore_affines,
				use_inv,                  // boolean             use_inv,
				skew_pull,                // double              skew_pull,
				tilt_pull,                // double              tilt_pull,
				scale_pull,               // double              scale_pull,
				position_pull,            // double              position_pull,
				overlap_pow,              // double              overlap_pow,
				rms_diff,                 // double              rms_diff,
				num_iter,                 // int                 num_iter,
				show_result,              // boolean             show_result,
				debugLevel);              // int                 debugLevel)		
	}		
		public static int buildOrthoMap(
				CLTParameters       clt_parameters,
				OrthoMapsCollection maps_collection,
				String              orthoMapsCollection_path,
				int [][]            available_pairs, 
				boolean             move_only,
				boolean             ignore_affines,
		        boolean             use_inv,
		        double              skew_pull,
		        double              tilt_pull,
		        double              scale_pull,
				double              position_pull,
		        double              overlap_pow,
				double              rms_diff,
				int                 num_iter,
				boolean             show_result,
				int                 debugLevel) {
		double []           val_coord =      null; // 1 - valid, 0 - invalid, minimize coordinates errors
		boolean             corr_avg= (skew_pull > 0) || (tilt_pull > 0) || (scale_pull > 0);
		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);
		
		OrthoMultiLMA oml = new OrthoMultiLMA(
				corr_avg,
				move_only);
		double lambda =            0.1;
		double lambda_scale_good = 0.5;
		double lambda_scale_bad =  8.0;
		double lambda_max =      100;
		boolean last_run =       false;
		oml.prepareLMA (
				clt_parameters,   // CLTParameters       clt_parameters,
				maps_collection,  // OrthoMapsCollection maps_collection,
				indices,          // int []              indices,
				defined_pairs, // int [][]            pairs, // or null? condensed
				ignore_affines,   // boolean             ignore_affines,
				val_coord,        // double []           val_coord, // 1 - valid, 0 - invalid, minimize coordinates errors
				position_pull,    // double              position_pull,
				skew_pull,        // double              skew_pull,
				tilt_pull,        // double              tilt_pull,
				scale_pull,       // double              scale_pull,
				use_inv,          // boolean             use_inv,
				overlap_pow,      // double              overlap_pow, // match weight as overlap fraction to this power
				debugLevel);      // int                 debugLevel)
		int lma_rslt = oml.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 = oml.getRms();
        double [] initial_rms = oml.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
        oml.updateAffines(maps_collection);
        if (show_result) {
        	double [][][] affines = oml.getAffines();
        	double [] fx = oml.getFx();

        	int         zoom_level = -2;
        	int [] wh = new int[2];
        	int [] origin = new int[2];
        	double [][] centers = new double [indices.length][];
        	double [][] dmulti = maps_collection.renderMultiDouble (
        			null, // double [][]   ground_planes, // null - images, non-null altitudes. use new double[2][3] for old way alt
        			indices,    // int []        indices, // null or which indices to use (normally just 2 for pairwise comparison)
        			true, // boolean       bounds_to_indices,
        			affines, // null, // affines,    // double [][][] affines, // null or [indices.length][2][3]
        			null,              // double [][]   equalize,
        			true,              // boolean       ignore_equalize,
        			null, // warp,       // FineXYCorr    warp,,
        			zoom_level, // int         zoom_level,
        			wh,         // int []      wh,
        			origin,     // int []      origin){ // maps[0] as a reference
        			centers);   // double [][] centers)
        	String [] titles = new String [dmulti.length];
        	for (int i = 0; i < dmulti.length; i++) {
        		titles[i] = indices[i]+"-"+maps_collection.ortho_maps[indices[i]].getName();
        	}

        	ImagePlus imp_multi = ShowDoubleFloatArrays.makeArrays(
        			dmulti,
        			wh[0],
        			wh[1],
        			"matched_multi.tiff",				
        			titles); // test_titles,
        	PointRoi roi = new PointRoi();
        	for (int i = 0; i < centers.length; i++) {
        		roi.addPoint(centers[i][0],centers[i][1], 1+i);
        	}
        	roi.setOptions("label");
        	imp_multi.setRoi(roi);
        	imp_multi.show();
        }

        if (orthoMapsCollection_path != null) {
        	try {
        		maps_collection.writeOrthoMapsCollection(orthoMapsCollection_path);
        	} catch (IOException e) {
        		// TODO Auto-generated catch block
        		e.printStackTrace();
        	}
        	if (debugLevel > -4) {
        		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,
			int [][]            pairs_pre, // or null?
			boolean             ignore_affines,
			double []           val_coord, // 1 - valid, 0 - invalid, minimize coordinates errors
			double              position_pull,
	        double              skew_pull,
	        double              tilt_pull,
	        double              scale_pull,
            boolean             use_inv,
            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>();
//		ArrayList<Double> offset_list = new ArrayList<Double>();
		if (pairs_pre != null) {
			for (int [] pair:pairs_pre) {
				int i = pair[0], j = pair[1];
				int scene0=indices[i],scene1=indices[j];
				PairwiseOrthoMatch match = maps_collection.ortho_maps[scene0].getMatch(maps_collection.ortho_maps[scene1].getName());
				matches[i][j] = match;
//				matches[j][i] = inv_match;
				pairs_list.add(new Point(i,j)); // only once?
			}
		} else {
			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) {
						matches[i][j] = match;
						matches[j][i] = inv_match;
						pairs_list.add(new Point(i,j)); // only once?
					}
				}
			}
		}
		num_pairs = pairs_list.size();
		offsets = new double [num_pairs][2];
		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};
			double [] enuOffset = maps_collection.ortho_maps[indices[pair.y]].enuOffsetTo(maps_collection.ortho_maps[indices[pair.x]]);
			offsets[np] = new double[] {enuOffset[0], -enuOffset[1]}; // {right,down} of the image 
		}
		pairs_ni = createNonIntersectingPairs(pairs);
		if (val_coord == null) { // maybe not needed - use weights
			val_coord = new double [indices.length];
			Arrays.fill(val_coord, 1.0);
		}
		
		int n62 = move_only ? 2 : 6;
		int n13 = move_only ? 1 : 3;
		
		y_vector =          new double [n62*num_pairs + 2 * num_scenes + (corr_avg ? 3:0)]; // last 2 * num_scenes + 3 will stay 0
		weights =           new double [n62*num_pairs + 2 * num_scenes + (corr_avg ? 3:0)];
		parameters_vector = new double [n62*num_scenes]; // maybe will need move-only mode?
		for (int n = 0; n < num_scenes; n++) {
			double [][] affine = ignore_affines? (new double[][] {{1,0,0},{0,1,0}}):maps_collection.ortho_maps[indices[n]].getAffine();
			System.arraycopy(affine[0], 0, parameters_vector, n62*n, n13);
			System.arraycopy(affine[1], 0, parameters_vector, n62*n + n13, n13);
			y_vector[n62*num_pairs+2*n + 0] = affine[0][2]; 
			y_vector[n62*num_pairs+2*n + 1] = affine[1][2]; 
		}
		double sw=0;
		for (int np = 0; np < num_pairs; np++) {
			PairwiseOrthoMatch match = matches[pairs[np][0]][pairs[np][1]];
			double [][] affine_pair = match.getAffine();
			if (move_only) {
				System.arraycopy(affine_pair[0], 2, y_vector, n62*np, 1);
				System.arraycopy(affine_pair[1], 2, y_vector, n62*np + 1, 1);
			} else {
				System.arraycopy(affine_pair[0], 0, y_vector, n62*np, 3);
				System.arraycopy(affine_pair[1], 0, y_vector, n62*np + 3, 3);
			}
			double overlap = match.overlap;
			double [][] jtj = match.jtj;
			double [] w = new double [n62];
			if (move_only) {
				w[0] = jtj[4][4]; w[1] = jtj[5][5];
			} else {			
				w[0] = jtj[0][0];w[1]=jtj[1][1]; w[2] = jtj[4][4]; // a00, a01, b0
				w[3] = jtj[2][2];w[4]=jtj[3][3]; w[5] = jtj[5][5]; // a10, a11, b1
			}
			double w_overlap = Math.pow(overlap, overlap_pow);
			for (int i = 0; i < w.length; i++) {
				w[i] = Math.sqrt(w[i]) * w_overlap;
				sw+=w[i];
			}
			System.arraycopy(w, 0, weights, n62*np, n62);
		}
		double swp = sw;
		int wi = n62 * num_pairs;
		for (int n = 0; n < num_scenes; n++) {
			double w = val_coord[n]*position_pull;
			weights[wi++] = w;
			weights[wi++] = w;
			sw+= 2*w;
		}
		if (corr_avg) {
			weights[wi++] = skew_pull;
			weights[wi++] = tilt_pull;
			weights[wi++] = scale_pull;
		}
		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 void  updateAffines(OrthoMapsCollection maps_collection) {
		for (int n = 0; n < num_scenes; n++) {
			double [][] affine = maps_collection.ortho_maps[indices[n]].getAffine();
			if (move_only) {
				System.arraycopy(parameters_vector, 2*n,     affine[0], 2,  1);
				System.arraycopy(parameters_vector, 2*n + 1, affine[1], 2,  1);
			} else {
				System.arraycopy(parameters_vector, 6*n,     affine[0], 0,  3);
				System.arraycopy(parameters_vector, 6*n + 3, affine[1], 0,  3);
			}
		}
	}
	public double [][][] getAffines(){
		double [][][] affines = new double [num_scenes][2][3];
		for (int n = 0; n < num_scenes; n++) {
			if (move_only) {
				affines[n][0][0] = 1.0;
				affines[n][1][1] = 1.0;
				System.arraycopy(parameters_vector, 2*n,     affines[n][0], 2,  1);
				System.arraycopy(parameters_vector, 2*n + 1, affines[n][1], 2,  1);

			} else {
				System.arraycopy(parameters_vector, 6*n,     affines[n][0], 0,  3);
				System.arraycopy(parameters_vector, 6*n + 3, affines[n][1], 0,  3);
			}
		}
		return affines;
	}
	
	
	
	public static double [][] getAffineAndDerivetives_delta(
			boolean       move_only,
			double        delta,
			double [][]   affine00,
			double [][]   affine10,
			double []     rd,
			double [][][] derivs) {
		double [][] affine = getAffineAndDerivatives(
				move_only, //boolean       move_only,
				affine00, // double [][]   affine0,
				affine10, // double [][]   affine1,
				rd,      // double []     rd,
				null); // double [][][] derivs)
		double [][][] affines0 = {affine00, affine10};
		for (int naff = 0; naff < affines0.length; naff++) {
			for (int row = 0; row < affines0[naff].length; row++) {
				for (int col = 0; col <  affines0[naff][row].length; col++) {
					double [][][] affines = {
							{affines0[0][0].clone(), affines0[0][1].clone()},
							{affines0[1][0].clone(), affines0[1][1].clone()}};
					affines[naff][row][col]= affines0[naff][row][col] - 0.5*delta;
					double [][] affine_minus = getAffineAndDerivatives(
							move_only, //boolean       move_only,
							affines[0], // double [][]   affine0,
							affines[1], // double [][]   affine1,
							rd,      // double []     rd,
							null); // double [][][] derivs)
					affines[naff][row][col]= affines0[naff][row][col] + 0.5*delta;
					double [][] affine_plus = getAffineAndDerivatives(
							move_only, //boolean       move_only,
							affines[0], // double [][]   affine0,
							affines[1], // double [][]   affine1,
							rd,      // double []     rd,
							null); // double [][][] derivs)
					double [][] daffine = new double [2][3];
					for (int i = 0; i < daffine.length; i++) {
						for (int j = 0; j < daffine[i].length; j++) {
							daffine[i][j] = (affine_plus[i][j] - affine_minus[i][j])/delta; 
						}
					}
					derivs[naff*6+row*3+col] = daffine;
				}
			}
		}
		return affine;
	}

	public static double [][] getAffineAndDerivatives(
			boolean       move_only,
			double [][]   affine0,
			double [][]   affine1,
			double []     rd,
			double [][][] derivs3) { // null or double[12] - will return affine derivatives by each of affine0, affine1
		double [] params = new double[move_only? 4 : 12];
		if (move_only) {
			params[0] = affine0[0][2];
			params[1] = affine0[1][2];
			params[2] = affine1[0][2];
			params[3] = affine1[1][2];
			
		} else {
			System.arraycopy(affine0[0], 0, params, 0, 3);
			System.arraycopy(affine0[1], 0, params, 3, 3);
			System.arraycopy(affine1[0], 0, params, 6, 3);
			System.arraycopy(affine1[1], 0, params, 9, 3);
		}
		double [][] derivs = (derivs3 !=null)? (new double[move_only? 4 : 12][]) : null;
		double [] l_affine= getAffineAndDerivatives(  // [2]/[6]
				move_only, //boolean       move_only,
				params,  // double []     params,
				0,       // int           pindx0,
				1,       // int           pindx1,
				rd,      // double []     rd,
				derivs); // double [][][] derivs)
		double [][] affine;
		if (move_only) {
			affine = new double [][]{
				{1,0,l_affine[0]},
				{0,1,l_affine[1]}};
				if (derivs3 !=null) {
					for (int i = 0; i < 4; i++) {
						derivs3[3*i+2] = new double [][] {
							{0,0,derivs[i][0]},
							{0,0,derivs[i][1]}};
					}
				}
		} else {
			affine = new double [][]{{l_affine[0],l_affine[1],l_affine[2]},{l_affine[3],l_affine[4],l_affine[5]}};
			if (derivs3 !=null) {
				for (int i = 0; i < derivs3.length; i++) {
					derivs3[i] = new double [][] {
						{derivs[i][0],derivs[i][1],derivs[i][2]},
						{derivs[i][3],derivs[i][4],derivs[i][5]}};
				}
			}
		}
		return affine;
	}
	
	public static double [] getAffineAndDerivatives(
			boolean       move_only,
			double []     params,
			int           pindx0,
			int           pindx1,
			double []     rd,
			double [][]   derivs) { // null or double[12] ([4] for move_only) - will return affine derivatives by each of affine0, affine1
		int indx0, indx1;
		double [][] aff0_a, aff0_b, aff1_a, aff1_b;
		
		if (move_only) {
			indx0 = 2 * pindx0;
			indx1 = 2 * pindx1;
			aff0_a = new double [][] {{1, 0},{0, 1}};
			aff0_b = new double [][] {{params[indx0+0]},{params[indx0+1]}};
			aff1_a = new double [][] {{1, 0},{0, 1}};
			aff1_b = new double [][] {{params[indx1+0]},{params[indx1+1]}};
		} else {
			indx0 = 6 * pindx0;
			indx1 = 6 * pindx1;
			aff0_a = new double [][] {{params[indx0+0],params[indx0+1]},{params[indx0+3],params[indx0+4]}};
			aff0_b = new double [][] {{params[indx0+2]},{params[indx0+5]}};
			aff1_a = new double [][] {{params[indx1+0],params[indx1+1]},{params[indx1+3],params[indx1+4]}};
			aff1_b = new double [][] {{params[indx1+2]},{params[indx1+5]}};
		}
		Matrix A0 = new Matrix (aff0_a);
		Matrix B0 = new Matrix (aff0_b);
		Matrix A1 = new Matrix (aff1_a);
		Matrix B1 = new Matrix (aff1_b);
		Matrix V =  new Matrix (new double [][] {{-rd[0]},{-rd[1]}});
		Matrix A =  A1.times(A0.inverse());
		Matrix B =  A.times(V.minus(B0)).minus(A1.times(V)).plus(B1);
		double [] affine =  move_only? (new double[] {B.get(0,0), B.get(1,0)}) :   
			(new double[] {A.get(0,0),A.get(0,1), B.get(0,0),	A.get(1,0),A.get(1,1), B.get(1,0)});
		if (derivs != null) {
			double [][] a = A.getArray();
			double [][] a0 = A0.getArray();
			double []   b0 = B0.getColumnPackedCopy();
			double [][] a1 = A1.getArray();
			double idet = 1/(a0[0][0]*a0[1][1]-a0[1][0]*a0[0][1]);
			double [] v = V.getColumnPackedCopy();
			double [] vb0= {v[0]-b0[0], v[1]-b0[1]};
			double d2 = -idet; // *idet; One idet is already included in a[][]
			double [][] didet_da = {{d2*a0[1][1], -d2*a0[1][0]}, {-d2*a0[0][1], d2*a0[0][0]}};
			/*
			 1/det0 * | a100, a101 | * | a011, -a001 |
			          | a110, a111 |   |-a010,  a000 |
			 * 
			 1/det0 * |a100*a011 - a101*a010, -a100*a001 + a101*a000 |
			          |a110*a011 - a111*a010, -a110*a001 + a111*a000 |
			 */
			double [][] da_da000 = {{0,  idet*a1[0][1]}, {0, idet*a1[1][1]}};
			double [][] da_da001 = {{0, -idet*a1[0][0]}, {0,-idet*a1[1][0]}};
			double [][] da_da010 = {{-idet*a1[0][1], 0}, {-idet*a1[1][1],0}};
			double [][] da_da011 = {{ idet*a1[0][0], 0}, { idet*a1[1][0],0}};
			double [][][][] da_da0 = {{da_da000, da_da001}, {da_da010, da_da011}};
			for (int i = 0; i < da_da0.length; i++) {
				for (int j = 0; j < da_da0[i].length; j++) {
					for (int k = 0; k < da_da0[i][j].length; k++) {
						for (int l = 0; l < da_da0[i][j][k].length; l++) {
							da_da0[i][j][k][l] += a[k][l]*didet_da[i][j]; // a[][] already has idet
						}
					}
				}
			}
			// some symmetry below - use to reduce calcs?
			double [][] da_da100 = {{ idet*a0[1][1],-idet*a0[0][1]},{0,0}};
			double [][] da_da101 = {{-idet*a0[1][0], idet*a0[0][0]},{0,0}};
			double [][] da_da110 = {{ 0,        0       },{ idet*a0[1][1],-idet*a0[0][1]}};   
			double [][] da_da111 = {{ 0,        0       },{-idet*a0[1][0], idet*a0[0][0]}}; 
			double [][][][] da_da1 = {{da_da100, da_da101}, {da_da110, da_da111}};
			double [][][][][] da_da = {da_da0, da_da1};
			double [] db_da000 = {
					da_da000[0][0]*vb0[0]+da_da000[0][1]*vb0[1],
					da_da000[1][0]*vb0[0]+da_da000[1][1]*vb0[1]};
			double [] db_da001 = {
					da_da001[0][0]*vb0[0]+da_da001[0][1]*vb0[1],
					da_da001[1][0]*vb0[0]+da_da001[1][1]*vb0[1]};
			double [] db_da010 = {
					da_da010[0][0]*vb0[0]+da_da010[0][1]*vb0[1],
					da_da010[1][0]*vb0[0]+da_da010[1][1]*vb0[1]};
			double [] db_da011 = {
					da_da011[0][0]*vb0[0]+da_da011[0][1]*vb0[1],
					da_da011[1][0]*vb0[0]+da_da011[1][1]*vb0[1]};
			double [][][] db_da0 = {{db_da000,db_da001},{db_da010,db_da011}};
			double [] db_da100 = {
					da_da100[0][0]*vb0[0]+da_da100[0][1]*vb0[1] - v[0],
					da_da100[1][0]*vb0[0]+da_da100[1][1]*vb0[1]};
			double [] db_da101 = {
					da_da101[0][0]*vb0[0]+da_da101[0][1]*vb0[1] - v[1],
					da_da101[1][0]*vb0[0]+da_da101[1][1]*vb0[1]};
			double [] db_da110 = {
					da_da110[0][0]*vb0[0]+da_da110[0][1]*vb0[1],
					da_da110[1][0]*vb0[0]+da_da110[1][1]*vb0[1] - v[0]};
			double [] db_da111 = {
					da_da111[0][0]*vb0[0]+da_da111[0][1]*vb0[1],
					da_da111[1][0]*vb0[0]+da_da111[1][1]*vb0[1] - v[1]};
			double [][][] db_da1 = {{db_da100,db_da101},{db_da110,db_da111}};
			double [][][][] db_da = {db_da0,db_da1};
			double [] db_db00 = {-a[0][0],-a[1][0]};
			double [] db_db01 = {-a[0][1],-a[1][1]};
			double [] db_db10 = {1,0};
			double [] db_db11 = {0,1};
			double [][][] db_db = {{db_db00,db_db01},{db_db10,db_db11}};
			if (move_only) {
				for (int naff = 0; naff < da_da.length; naff++) {
					for (int row = 0; row < da_da[naff].length; row++) {
						int indx = naff*2+row;
						derivs[indx] = new double[2]; // [2][3];
						for (int i = 0; i < 2; i++) {
							derivs[indx][i] = db_db[naff][row][i];
						}							
					}
				}
			} else {
				// fill in  derivatives, skipping B - only 2x2 A
				for (int naff = 0; naff < da_da.length; naff++) {
					for (int row = 0; row < da_da[naff].length; row++) {
						for (int col = 0; col <  3; col++) {
							int indx = naff*6+row*3+col;
							derivs[indx] = new double[6]; // [2][3];
							if ((col <  da_da[naff][row].length) ) {
								for (int i = 0; i < 2; i++) {
									for (int j = 0; j< 2; j++) {
										derivs[indx][3 * i + j] = da_da[naff][row][col][i][j];
									}
									derivs[indx][3 * i + 2] = db_da[naff][row][col][i];
								}
							} else { // d/db
								for (int i = 0; i < 2; i++) {
									derivs[indx][3 * i + 2] = db_db[naff][row][i];
								}							
							}
						}
					}
				}
			}
		}
		return affine; // _alt;
	}
	
	public static int [][] createNonIntersectingPairs(int [][] pairs) {
		ArrayList<Integer> pairs_list = new ArrayList<Integer>();
		for (int i = 0; i < pairs.length; i++) {
			pairs_list.add(i);
		}
		int max_scene = 0;
		for (int [] pair:pairs) {
			max_scene=Math.max(max_scene, pair[0]);
			max_scene=Math.max(max_scene, pair[1]);
		}
		ArrayList<ArrayList<Integer>> list_list_pairs = new ArrayList<ArrayList<Integer>>();
		while (!pairs_list.isEmpty()) {
			boolean [] used = new boolean [max_scene+1];
			ArrayList<Integer> pairs_new = new ArrayList<Integer>();
			for (int i = 0; i < pairs_list.size(); ) { // no increment here!
				int ip = pairs_list.get(i);
				int [] p = pairs[ip];
				if (used[p[0]] || used[p[1]]) {
					i++;
				} else {	
					used[p[0]] = true;
					used[p[1]] = true;
					pairs_new.add(ip);
					pairs_list.remove(i);
				}
			}
			list_list_pairs.add(pairs_new);		
		}
		int [][] pairs_ni = new int [list_list_pairs.size()][];
		for (int n = 0; n < pairs_ni.length; n++) {
			pairs_list = list_list_pairs.get(n);
			pairs_ni[n] = new int [pairs_list.size()];
			for (int i = 0;i < pairs_ni[n].length; i++) {
				pairs_ni[n][i] = pairs_list.get(i);
			}
		}
		return pairs_ni;
 	}
	
	
	

	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]) {// the place to put a breakpoint
				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 int n62 = move_only ? 2 : 6; 
		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 np6 = n62*nPair;
						for (int i1 = 0; i1 < n62; i1++) {
							int i = np6+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 = n62*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;
	}	
	
	private double [] getFxDerivs(
			final double []   vector,
			final double [][] jt, // should be null or initialized with [vector.length][]
			final int         debug_level)
	{
		final int n62 = move_only ? 2 : 6;    
		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() {
						double [][] jt_frac = (jt != null)? (new double [2 * n62][]):null;
						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 [] fx_frac = getAffineAndDerivatives(
									move_only,
									vector,         // double []     params,
									pair[0],        // int           pindx0,
									pair[1],        // int           pindx1,
									offsets[nPair], // double []     rd,
									jt_frac);       // double [][]  derivs)
							System.arraycopy(
									fx_frac,
									0,
									fx,
									n62 * nPair,
									n62);
							if (jt_frac != null) {
								for (int ns = 0; ns < 2; ns++) {
									for (int nsp = 0; nsp < n62; nsp++) {
										for (int i = 0; i < n62; i++) {
											jt[n62 * pairs[nPair][ns] + nsp][n62 * nPair + i] +=
													jt_frac[n62 * ns +nsp][i];
										}
									}
								}
							}
						}
					}
				};
			}		      
			ImageDtt.startAndJoin(threads);
			ai.set(0);
		}
		final int pull_indx = n62 * num_pairs;
		final int [] ioffs = {move_only? 0:2,move_only? 1:5};
		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()){
						fx[pull_indx + 2 * nScene + 0] = vector[n62 * nScene + ioffs[0]];
						fx[pull_indx + 2 * nScene + 1] = vector[n62 * nScene + ioffs[1]];
						if (jt!= null) {
							jt[n62 * nScene + ioffs[0]][pull_indx + 2 * nScene + 0] = 1.0;
							jt[n62 * nScene + ioffs[1]][pull_indx + 2 * nScene + 1] = 1.0;
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		ai.set(0);
		if (corr_avg) { //  && ((skew_pull > 0) || (tilt_pull > 0) || (scale_pull > 0))) {
			final int pull_indices = weights.length-3;
			for (int nScene = 0; nScene < num_scenes; nScene++){
				int indx = 6 * nScene;
				double a00 = vector[indx+0],a01=vector[indx+1],a10=vector[indx+3],a11=vector[indx+4];
				double aa1 = 0.5*(a00*a00 + a10*a10);
				double aa2 = 0.5*(a01*a01 + a11*a11);
				fx[pull_indices + 0] += a00*a01+a10*a11;
				fx[pull_indices + 1] += aa1-aa2;
				fx[pull_indices + 2] += aa1+aa2 - 1.0;
			}
			
			if (jt!= null) {
				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 indx = 6 * nScene;
								double a00=vector[indx+0],a01=vector[indx+1],a10=vector[indx+3],a11=vector[indx+4];
								jt[indx + 0][pull_indices + 0] = a01;
								jt[indx + 1][pull_indices + 0] = a00;
								jt[indx + 3][pull_indices + 0] = a11;
								jt[indx + 4][pull_indices + 0] = a10;
								
								jt[indx + 0][pull_indices + 1] = a00;
								jt[indx + 1][pull_indices + 1] =-a01;
								jt[indx + 3][pull_indices + 1] = a10;
								jt[indx + 4][pull_indices + 1] =-a11;
								
								jt[indx + 0][pull_indices + 2] = a00;
								jt[indx + 1][pull_indices + 2] = a01;
								jt[indx + 3][pull_indices + 2] = a10;
								jt[indx + 4][pull_indices + 2] = a11;
							}
						}
					};
				}		      
				ImageDtt.startAndJoin(threads);
				ai.set(0);
			}
		}
		
		return fx;
	}	

}
