/**
 **
 ** CuasMotionLMA.java - LMA solver for UAS target position and motion fitting
 **
 ** Copyright (C) 2025-2026 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  CuasMotionLMA.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.cuas;

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

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

import Jama.Matrix;

/*
 * Use fixed-position relative to initial max gaussian window weight
 * fx = A*cos2(r/R0*pi/2)*cos(K*r/R0*pi/2)+C
 * K<1, K~=2
 * A,K,R0,C
 * RR0= pi/(R0*2)
 * r=sqrt((x-X0)^2+(y-Y0)^2)
 * 
 * fx = A*cos2(r*RR0)*cos(K*RR0*r)+C
 * 
 *  
 *   
 * 
 */



public class CuasMotionLMA {
	private static final double R_OFFS = 1e-6;
	public static final int INDX_A =   0;
	public static final int INDX_C =   1;
	public static final int INDX_RR0 = 2;
	public static final int INDX_K =   3;
	public static final int INDX_X0 =  4; // relative to left
	public static final int INDX_Y0 =  5; // relative to top
	public static final int INDX_LEN = INDX_Y0+1;
	
	// result vector indices
	public static final int RSLT_X =       0;
	public static final int RSLT_Y =       1;
	public static final int RSLT_A =       2;
	public static final int RSLT_R0 =      3;
	public static final int RSLT_R1 =      4;
	public static final int RSLT_K =       5;
	public static final int RSLT_C =       6;
	public static final int RSLT_RMS =     7;
	public static final int RSLT_RMS_A =   8;
	public static final int RSLT_MAX2A =   9;
	public static final int RSLT_ITERS =  10;
	public static final int RSLT_CENTERED=11; //1: this target was evaluated in centered mode, 0 - was in non-centered, -1 - consumed in non-centered
// add	
	public static final int RSLT_EBOOST=  12; // target accumulated with boosted "exposure time" (based scaled this) 
	public static final int RSLT_CENT_X = 13;
	public static final int RSLT_CENT_Y = 14;
	public static final int RSLT_CENT_MX= 15;
	public static final int RSLT_CENT_F = 16;
	
	public static final int RSLT_VX =     17;
	public static final int RSLT_VY =     18;
	public static final int RSLT_VSTR =   19;
	public static final int RSLT_VFRAC =  20;
	public static final int RSLT_VBOOST = 21; // motion vector was calculated with boosted number of accumulated scene pairs (based scaled this)
	public static final int RSLT_VCORR =  22; // Last correction of VX, VY during refine (large correction means failed correction)
	public static final int RSLT_BX =     23;
	public static final int RSLT_BY =     24; // RSLT_BX+1;
	public static final int RSLT_AX =     25; // RSLT_BX+2;
	public static final int RSLT_AY =     26; // RSLT_BX+3;
	public static final int RSLT_MISMATCH_BEFORE = 27;
	public static final int RSLT_MISMATCH_AFTER =  28; // RSLT_MISMATCH_BEFORE+1;
	public static final int RSLT_MISMATCH_DIRS=    29;
	public static final int RSLT_MATCH_LENGTH=     30; // is one less than the total length (0 - isolated)
	public static final int RSLT_BEFORE_LENGTH=    31; // just for debug
	public static final int RSLT_AFTER_LENGTH=     32; // just for debug
	public static final int RSLT_SEQ_TRAVEL=       33; 
	public static final int RSLT_MSCORE =          34;
	public static final int RSLT_QA =              35;
	public static final int RSLT_QRMS =            36;
	public static final int RSLT_QRMS_A =          37;
	public static final int RSLT_QCENTER =         38;
	public static final int RSLT_QMATCH =          39;
	public static final int RSLT_QMATCH_LEN=       40;
	public static final int RSLT_QTRAVEL=          41;
	public static final int RSLT_QSCORE =          42;
	public static final int RSLT_STRONGER =        43; // index of stronger neighbor (may be more)
	public static final int RSLT_SLOW =            44; // 1 - slow, 0 - fast 
	public static final int RSLT_WHEN =            45;
	public static final int RSLT_FAIL =            46;
	public static final int RSLT_DISPARITY=        47; // disparity from single keyframe sequences
	public static final int RSLT_DISP_DIFF=        48; // disparity difference between what?
	public static final int RSLT_DISP_STR =        49; // disparity strength
	public static final int RSLT_RANGE =           50; // range (meters) found from RSLT_DISPARITY, modified by RSLT_INFINITY
	public static final int RSLT_GLOBAL =          51; // local target ID (1-based)
	public static final int RSLT_GLENGTH =         52; // length (symmetrical around this one) for which disparity and range are provided
	public static final int RSLT_GDISPARITY=       53; // disparity from accumulation of the range (RSLT_GLENGTH)
	public static final int RSLT_GDISP_DIFF=       54; // disparity difference for a range
	public static final int RSLT_GDISP_STR =       55; // disparity strength for a range
	public static final int RSLT_GRANGE =          56; // global range - calculated for long sequences (and its fractions)
	public static final int RSLT_FL_PX =           57; // flight log px
	public static final int RSLT_FL_PY =           58; // flight log py
	public static final int RSLT_FL_DISP =         59; // flight log true disparity
	public static final int RSLT_FL_RANGE =        60; // flight log range (meters)
	public static final int RSLT_INFINITY =        61; // disparity at infinity used for range calculation
	public static final int RSLT_TARGET_ID =       62; // unique target id for the whole sequence of segments. 1 is reserved for the UAS
	public static final int RSLT_VEL_AWAY =        63; // axial velocity (positive - away), range derivative (m/s), calculated from the difference to the previous/next series range 
	public static final int RSLT_VEL_RIGHT =       64; // lateral velocity, right ( calculated from angular and RSLT_GRANGE
	public static final int RSLT_VEL_UP =          65; // lateral velocity, up ( calculated from angular and RSLT_GRANGE
	public static final int RSLT_RANGE_LIN =       66; // range linear interpolated using previous/next series if the same target is present there
	public static final int RSLT_PHASE1_INDEX =    67; // target index in phase 1 (to trim) - debug feature 
	public static final int RSLT_PHASE1_SCORE =    68; // target index in phase 1 (to set minimal score for phase 1) - debug feature 
	public static final int RSLT_PHASE1_AMP =      69; // target index in phase 1 (to set minimal score for phase 1) - debug feature 
	public static final int RSLT_PHASE1_MSCORE =   70; // target index in phase 1 (to set minimal score for phase 1) - debug feature 
	public static final int RSLT_PHASE1_VSTR =     71; // target index in phase 1 (to set minimal score for phase 1) - debug feature 
	public static final int RSLT_PHASE1_VBOOST =   72; // target index in phase 1 (to set minimal score for phase 1) - debug feature 
	public static final int RSLT_PHASE1_EBOOST =   73; // target index in phase 1 (to set minimal score for phase 1) - debug feature 
	public static final int RSLT_LEN = RSLT_PHASE1_EBOOST + 1;
	
	public static final String [] LMA_TITLES = 
		{"X-OFFS","Y-OFFS", "AMPLITUDE", "RADIUS","RAD_POS", "OVERSHOOT","OFFSET","RMSE","RMSE/A","MAX2A","ITERATIONS",
				"CENTERED", "EXP-BOOST",
				"Centr-X","Centr-Y","Centr-max","Centr-frac",
				"Vx", "Vy", "V-conf","V-frac", "V-boost","V-corr",// from motion vectors
				"X-before", "Y-before","X-after","Y-after",  // from getHalfBeforeAfterPixXY()
				"ERR-BEFORE", "ERR-AFTER", "BA-DIRS", // before dir + 16*after dir
				"Match-length",
				"Length-before","Length-after", // just for debug, may be removed later
				"TRAVEL",
				"*MOTION-SCORE",
				"*Q-AMPL","*Q-RMSE","*Q-RMSE/A","*Q-CENTER","*Q-MATCH","*Q-LENGTH","*QTRAVEL","*Q-SCORE",
				"Stronger","Slow",
				"WHEN", "FAILURE",
				"Disparity","Disparity-Diff","Strength","Range","Global-index",
				"segment-length","segment-disparity","segment_disp-diff", "segment-strength", "segment-range",
				"FLOG-px","FLOG-pY","FLOG-DISP","FLOG-range","infinity",
				"GTarget-ID","Vel-away", "Vel-right", "Vel-up","Range-linear",
				"Phase1-index","Phase1-score","Phase1-amplitude","Phase1-mscore","Phase1-vstr",
				"Phase1-vboost", "Phase1-exp_boost"};
	public static final String EXTRA_SLICE_DISCARD_ON_LOAD = "Targets"; 
	public static final int FAIL_PENDING =  -1; // By Claude on 05/07/2026: motion sentinel — tile tracked but not yet processed by LMA
	public static final int FAIL_NONE =      0;
	public static final int FAIL_MOTION =    1;  // motion strength/fraction too low, also - motion score too low
	public static final int FAIL_NO_MAX =    2;  // no suitable local maximum 
	public static final int FAIL_CENT_STR =  3;  // centroid amplitude is too low
	public static final int FAIL_CENT_FRAC = 4;  // centroid fraction (energy in the peak fraction of all) is too low
	public static final int FAIL_LMA =       5;  // LMA fail to converge
	public static final int FAIL_A_PRE =     6;  // amplitude is too low at preliminary filter (just after LMA)
	public static final int FAIL_A_LOW =     7;  // amplitude is too low
	public static final int FAIL_Q_LOW =     8;  // score too low
	public static final int FAIL_ACENT =     9;  // ratio of maximal pixel to amplitude is too low
	public static final int FAIL_RMSE =     10;  // RMSE is too high
	public static final int FAIL_RMSE_R =   11;  // BOTH RMSE is not sufficient and RMSE/A is too high
	public static final int FAIL_R0_HIGH =  12;  // Full radius (including negative overshoot) is too high
	public static final int FAIL_R1_LOW =   13;  // Inner (positive) peak radius is too low
	public static final int FAIL_K_LOW =    14;  // Overshoot is too low (not used, it can be down to 0)
	public static final int FAIL_K_HIGH =   15;  // Overshoot is too high
	public static final int FAIL_FAR =      16;  // Peak is too far from the center
	public static final int FAIL_BOOSTS=   17;  // Tried to use short exposure for slow motion   
	public static final int FAIL_VCORR =    18;  // MV refinement (fine pass) resulted in a too high correction (not yet checked)
	public static final int FAIL_HORIZON =  19;  // Peak is below horizon 
	public static final int FAIL_MISMATCH = 20;  // Mismatch on both ends is too high 
	public static final int FAIL_NEIGHBOR = 21;  // failed because some neighbor is stronger   
	public static final int FAIL_DUPLICATE= 22;  // coordinate are (almost) the same as those of a stronger tile   
	public static final int FAIL_USED=      23;  // non-centered used to generate centered, remove this   
	public static final int FAIL_FL_ONLY=   24;  // Flight log data only, no target detected here   
	
	public static final int CENTERED_NO =    0; 
	public static final int CENTERED_YES =   1; 
	public static final int CENTERED_USED=  -1; 
	
	private int          width;
	private double [][]  window;
	private double       pedestal;
	private double []    y_vector;
	private double []    weights;
	private double []    full_vector = new double[INDX_LEN];
	private int []       pindx; // full parameter index for vector
	private int []       rindx; // vector index or -1
	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 []    last_ymfx =       null;	
	private double [][]  last_jt =         null;
	private int          iters =           -2; // never ran
	private double       max_val = 0;

	public CuasMotionLMA(
			int         width,
			double     sigma,
			double     wnd_pedestal) {
		this.width = width;
		this.pedestal = wnd_pedestal;
		window = new double [width][width];
		double k = -0.5/(sigma*sigma);
		for (int i = 0; i < width; i++) {
			for (int j = 0; j < width; j++) {
				window[i][j] = Math.exp(k*(i*i+j*j));//  + wnd_pedestal; // will be normalized when used
			}
		}
	}
	public int prepareLMA(
			boolean [] param_select,
			double []  tile_data,
			double     xc, // relative to center =width/2
			double     yc, // relative to center =width/2
			double     r0,
			double     k,
			double     lmax_val, // maximal pixel value near the centroid maximum to be used for comparison with A
			int        debugLevel) {
		max_val = lmax_val;
		y_vector = tile_data;
		double x0 = Math.min(Math.max(xc + width/2, 0), width-1);
		double y0 = Math.min(Math.max(yc + width/2, 0), width-1);
		

		int ix0 = (int) Math.round(x0);
		int iy0 = (int) Math.round(y0);
		full_vector[INDX_A] = tile_data[ix0+iy0*width];
		full_vector[INDX_C] = 0;
		full_vector[INDX_RR0] = Math.PI/(2* r0);
//		full_vector[INDX_K] = k; // > 1 (~2.0)
		full_vector[INDX_K] = Math.sqrt(Math.abs(k-1)); // > 1 (~2.0) now k = 1 + k2*k2;
		full_vector[INDX_X0] = x0;
		full_vector[INDX_Y0] = y0;
		weights = new double [width*width];
		double sw = 0;
		for (int y = 0; y < width; y++) {
			int ay = Math.abs(y-iy0);
			for (int x = 0; x < width; x++) {
				int ax = Math.abs(x-ix0);
				double w = window[ay][ax]+pedestal; // window to the nearest integer x,y
				weights[x + y*width] = w;
				sw += w;
			}
		}
		for (int i = 0; i < weights.length; i++) {
			weights[i] /= sw;
		}
		int indx = 0;
		for (int i = 0; i < INDX_LEN; i++) if (param_select[i]){
			indx++;
		}
		rindx = new int [INDX_LEN];
		pindx = new int [indx];
		indx= 0;
		for (int i = 0; i < INDX_LEN; i++) {
			if (param_select[i]) {
				pindx[indx] = i;
				rindx[i] = indx++;
			} else {
				rindx[i] = -1;
			}
		}
   		last_jt = new double [pindx.length][];
		
		double [] fx = getFxDerivs(
				getParametersVector(), // double []         vector,
				last_jt,               // final double [][] jt, // should be null or initialized with [vector.length][]
				debugLevel);           // final int         debug_level)
		last_rms =        new double [2];
		last_ymfx = getYminusFxWeighted(
				fx, // final double []   fx,
				last_rms); // final double []   rms_fp // null or [2]
		
		initial_rms =     last_rms.clone();
		good_or_bad_rms = last_rms.clone();
		return 0;
	}	

	public int prepareLMA_orig(
			boolean [] param_select,
			double []  tile_data,
			double     xc, // relative to center =width/2
			double     yc, // relative to center =width/2
			double     r0,
			double     k,
			double     lmax_val, // maximal pixel value near the centroid maximum to be used for comparison with A
			int        debugLevel) {
		max_val = lmax_val;
		y_vector = tile_data;
		double x0 = Math.min(Math.max(xc + width/2, 0), width-1);
		double y0 = Math.min(Math.max(yc + width/2, 0), width-1);
		

		int ix0 = (int) Math.round(x0);
		int iy0 = (int) Math.round(y0);
		full_vector[INDX_A] = tile_data[ix0+iy0*width];
		full_vector[INDX_C] = 0;
		full_vector[INDX_RR0] = Math.PI/(2* r0);
		full_vector[INDX_K] = k; // > 1 (~2.0)
		full_vector[INDX_X0] = x0;
		full_vector[INDX_Y0] = y0;
		weights = new double [width*width];
		double sw = 0;
		for (int y = 0; y < width; y++) {
			int ay = Math.abs(y-iy0);
			for (int x = 0; x < width; x++) {
				int ax = Math.abs(x-ix0);
				double w = window[ay][ax]+pedestal; // window to the nearest integer x,y
				weights[x + y*width] = w;
				sw += w;
			}
		}
		for (int i = 0; i < weights.length; i++) {
			weights[i] /= sw;
		}
		int indx = 0;
		for (int i = 0; i < INDX_LEN; i++) if (param_select[i]){
			indx++;
		}
		rindx = new int [INDX_LEN];
		pindx = new int [indx];
		indx= 0;
		for (int i = 0; i < INDX_LEN; i++) {
			if (param_select[i]) {
				pindx[indx] = i;
				rindx[i] = indx++;
			} else {
				rindx[i] = -1;
			}
		}
   		last_jt = new double [pindx.length][];
		
		double [] fx = getFxDerivs(
				getParametersVector(), // double []         vector,
				last_jt,               // final double [][] jt, // should be null or initialized with [vector.length][]
				debugLevel);           // final int         debug_level)
		last_rms =        new double [2];
		last_ymfx = getYminusFxWeighted(
				fx, // final double []   fx,
				last_rms); // final double []   rms_fp // null or [2]
		
		initial_rms =     last_rms.clone();
		good_or_bad_rms = last_rms.clone();
		return 0;
	}	
	
	public double getRMS() {
		return last_rms[0];
	}
	
	public double getInitialRMS() {
		return initial_rms[0];
	}
	
	
	public static double [] getEmpty() {
		double rslt[] = new double [RSLT_LEN];
		Arrays.fill(rslt, Double.NaN);
		return rslt;
	}

	public void setResult(
			double [] rslt) {
		rslt[RSLT_X] =               getCenter()[0];
		rslt[RSLT_Y] =               getCenter()[1];
//		rslt[RSLT_A] =               getA();
		rslt[RSLT_A] =               getMaxFx(); // maximum of fx, getA() may be much larger!
		rslt[RSLT_R0] =              getR0();
		rslt[RSLT_R1] =              getR0() / ((getK()>1) ? getK() : 1.0);
		rslt[RSLT_K] =               getK();
		rslt[RSLT_C] =               getC();
		rslt[RSLT_RMS] =             getRMS();
		rslt[RSLT_RMS_A] =           getRMS()/rslt[RSLT_A]; // getA();
		rslt[RSLT_MAX2A] =           max_val/rslt[RSLT_A]; // getA(); // ratio of maximal value to LMA amplitude 
		rslt[RSLT_ITERS] =           getIters();
		return;
	}
	public double [] getCenter(){
		return new double [] {full_vector[INDX_X0] - width/2, full_vector[INDX_Y0]-width/2};
	}
	public double getR0() {
		return Math.PI/(2 * full_vector[INDX_RR0]);
	}

	public double getA() {
		return full_vector[INDX_A];
	}

	public double getC() {
		return full_vector[INDX_C];
	}
	
	public double getK() {
		return 1 + full_vector[INDX_K]*full_vector[INDX_K];
	}

	public double getK_orig() {
		return full_vector[INDX_K];
	}
	
	public int getIters() {
		return iters;
	}
	public double [] getParametersVector() {
		double [] vector = new double [pindx.length];
		for (int i = 0; i < vector.length; i++) {
			vector[i] = full_vector[pindx[i]];
		}
		return vector;
	}
	public void setParametersVector(double [] vector) {
		for (int i = 0; i < vector.length; i++) {
			full_vector[pindx[i]] = vector[i];
		}
	}
	
	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
			int    dbg_seq,
			int    dbg_tile,
			int    dbg_index,
			int    debug_level)
	{
		boolean last_run = true;
		boolean [] rslt = {false,false};
		this.last_rms = null; // remove?
		int iter = 0;
		for (iter = 0; iter < num_iter; iter++) {
			rslt =  lmaStep(
					lambda,
					rms_diff,
					debug_level);
			if (rslt == null) {
				iters = -1;
				return iters;
			}
			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 (dbg_prefix != null) {
//				showDebugImage(dbg_prefix+"-"+iter+(rslt[0]?"-GOOD":"-BAD"));
//			}

		}
		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");
			}
		}
//		if (dbg_prefix != null) {
//			showDebugImage(dbg_prefix+"-FINAL");
//		}
		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){ 
//			String [] lines1 = printOldNew(false); // boolean allvectors)
//			System.out.println("iteration="+iter);
//			for (String line : lines1) {
//				System.out.println(line);
//			}
		}
		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);
				}
//				String [] lines = printOldNew(false); // boolean allvectors)
//				for (String line : lines) {
//					System.out.println(line);
//				}
			}
		}
		if ((debug_level > -2) && !rslt[0]) { // failed
			if ((debug_level > 1) || (iter == 1) || last_run) {
				System.out.println("LMA failed on iteration = "+iter+ ", nSeq="+dbg_seq+", ntile="+dbg_tile+((dbg_index>=0)?(", index="+dbg_index):""));
//				String [] lines = printOldNew(true); // boolean allvectors)
//				for (String line : lines) {
//					System.out.println(line);
//				}
			}
			// System.out.println();
		}
		// No need to updateFullParameters() in this implementation - they are already updated
		/*
		if (rslt[0]) {
			updateFullParameters();
		}
		*/
		iters = rslt[0]? iter : -1;
		return iters;
	}

	
	
	private boolean [] lmaStep(
			double lambda,
			double rms_diff,
			int debug_level) {
		boolean [] rslt = {false,false};
		double [] parameters_vector = getParametersVector();
		// 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, // 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 (last_ymfx == null) {
				return null; // need to re-init/restart LMA
			}
			// TODO: Restore/implement
			if (debug_level > 3) {
				double    delta = 1E-5;
			 	double delta_err=compareJT(
			 			parameters_vector, // double [] vector,
						delta,             // double    delta,
						false); // last3only);        // boolean   last3only); // do not process samples - they are tested before
				System.out.println("\nMaximal error = "+delta_err);
				
				/*
				 dbgJacobians(
							corr_vector, // GeometryCorrection.CorrVector corr_vector,
							1E-5, // double delta,
							true); //boolean graphic)
				*/
			}
		}
		
		if (debug_level > 3) {
			double    delta = 1E-5;
		 	double delta_err=compareJT(
		 			parameters_vector, // double [] vector,
					delta,             // double    delta,
					false); // last3only);        // boolean   last3only); // do not process samples - they are tested before
			System.out.println("\nMaximal error = "+delta_err);
			
			/*
			 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, 10);
		}

		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(
				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();
			parameters_vector = new_vector.clone();
			setParametersVector(new_vector);
			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( // USED in lwir
			final double      lambda,
			final double [][] jt)
	{
		final int num_pars = jt.length;
		final int nup_points = jt[0].length;
		final double [][] wjtjl = new double [num_pars][num_pars];
		for (int i = 0; i < num_pars; i++) {
			for (int j = i; j < num_pars; j++) {
				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];
					if (Double.isNaN(d)) {
						System.out.println("CuasMotionLMA.getWJtJlambda(): NAN i="+i+", j="+j+", k="+k);
					}
				}
				wjtjl[i][j] = d;
				if (i == j) {
					wjtjl[i][j] += d * lambda;
				} else {
					wjtjl[j][i] = d;
				}
			}
		}
		return wjtjl;
	}
	
	
	
	private double [] getYminusFxWeighted( // problems. at least with eigen?
			final double []   fx,
			final double []   rms_fp) { // null or [2]
		final double []     wymfw =       new double [fx.length];
		double s_rms = 0.0;
		for (int i = 0; i < fx.length; i++) {
			double d = y_vector[i] - fx[i];
			double wd = d * weights[i];
			wymfw[i] = wd;
			s_rms += d * wd;
		}
		double rms = Math.sqrt(s_rms); // assuming sum_weights == 1.0; /pure_weight); they should be re-normalized after adding regularization
		if (rms_fp != null) {
			rms_fp[0] = rms;
			if (rms_fp.length > 1) {
				rms_fp[1] = rms; // _pure;
			}
		}
		return wymfw;
	}
	
 	private double compareJT(
			double [] vector,
			double    delta,
			boolean   last3only) { // do not process samples - they are tested before
		double []  errors=new double [vector.length];
		double [][] jt =  new double [vector.length][];
		System.out.print("Parameters vector = [");
		for (int i = 0; i < vector.length; i++) {
			System.out.print(vector[i]);
			if (i < (vector.length -1)) System.out.print(", ");
		}
		System.out.println("]");
		getFxDerivs(
				vector,
				jt, // final double [][] jt, // should be null or initialized with [vector.length][]
				1); // debug_level);
		double [][] jt_delta =  getFxDerivsDelta(
				vector, // double []         vector,
				delta, // final double      delta,
				-1); // final int         debug_level)
		int start_index = last3only? (weights.length-3) : 0;
		for (int n = start_index; n < weights.length; n++) if (weights[n] > 0) {
			System.out.print(String.format("%3d",n));
			for (int i = 0; i < vector.length; i++) {
				System.out.print(String.format("\t%12.9f",jt[i][n]));
			}			
			for (int i = 0; i < vector.length; i++) {
				System.out.print(String.format("\t%12.9f",jt_delta[i][n]));
			}			
			for (int i = 0; i < vector.length; i++) {
				System.out.print(String.format("\t%12.9f",jt[i][n]-jt_delta[i][n]));
			}			
			System.out.println();
			/*
			System.out.println(String.format(
					"%3d\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f",
					n, jt[0][n], jt[1][n], jt[2][n], jt[3][n],
					jt_delta[0][n], jt_delta[1][n], jt_delta[2][n], jt_delta[3][n],
					jt[0][n]-jt_delta[0][n],jt[1][n]-jt_delta[1][n],jt[2][n]-jt_delta[2][n],jt[3][n]-jt_delta[3][n]));
					*/
			for (int i = 0; i < vector.length; i++) {
				errors[i] = Math.max(errors[i], jt[i][n]-jt_delta[i][n]);
			}
		}
		for (int i = 0; i < vector.length; i++) {
			System.out.print("\t\t");
		}			
		for (int i = 0; i < vector.length; i++) {
			System.out.print(String.format("\t%12.9f",errors[i]));
		}			
        /*
		System.out.println(String.format(
				"-\t-\t-\t-\t-\t-\t-\t-\t-\t%12.9f\t%12.9f\t%12.9f\t%12.9f",
				errors[0], errors[1], errors[2], errors[3]));
				*/
		double err=0;
		for (int i = 0; i < vector.length; i++) {
			err = Math.max(errors[i], err);
		}
		return err;
	}
	
 	
	private double [][] getFxDerivsDelta(
			double []         vector,
			final double      delta,
			final int         debug_level) {
		double [][] jt =  new double [vector.length][weights.length];
		for (int nv = 0; nv < vector.length; nv++) {
			double [] vpm = vector.clone();
			vpm[nv]+= 0.5*delta;
			double [] fx_p =  getFxDerivs(
					vpm,
					null, // final double [][] jt, // should be null or initialized with [vector.length][]
					debug_level);
			vpm[nv]-= delta;
			double [] fx_m =  getFxDerivs(
					vpm,
					null, // final double [][] jt, // should be null or initialized with [vector.length][]
					debug_level);
			for (int i = 0; i < weights.length; i++) if (weights[i] > 0) {
				jt[nv][i] = (fx_p[i]-fx_m[i])/delta;
			}
		}
		return jt;
	}

	public double getMaxFx() {
		double [] fx = getFx();
		double mfx = Double.NaN;
		for (double d:fx) {
			if (!(d <= mfx)) {
				mfx = d;
			}
		}
		return mfx;
	}
	public double [] getFx() {
		return getFxDerivs(
				full_vector, // final double []   vector,
				null,        // final double [][] jt, // should be null or initialized with [vector.length][]
				-10);        // final int         debug_level)
	}
	public double [] getLastYMinusFx() {
		return last_ymfx;
	}
	
	public double [] getWeights() {
		return weights;
	}
	
	private double [] getFxDerivs(
			final double []   vector,
			final double [][] jt, // should be null or initialized with [vector.length][]
			final int         debug_level) {
		int height = y_vector.length/width;
		final double [] fx = new double [y_vector.length];
		if (jt != null) {
			for (int i = 0; i < jt.length; i++) {
				jt[i] = new double [y_vector.length]; // weights.length];
			}
		}
		double A=    (rindx[INDX_A] >=0)?   vector[rindx[INDX_A]] :   full_vector[INDX_A] ; 
		double C=    (rindx[INDX_C] >=0)?   vector[rindx[INDX_C]] :   full_vector[INDX_C] ;
		double RR0 = (rindx[INDX_RR0] >=0)? vector[rindx[INDX_RR0]] : full_vector[INDX_RR0] ;
//		double K =   (rindx[INDX_K] >=0)?   vector[rindx[INDX_K]] :   full_vector[INDX_K] ; // >1.0
		double K2 =  (rindx[INDX_K] >=0)?   vector[rindx[INDX_K]] :   full_vector[INDX_K] ; // >1.0
		double K =   1+ K2*K2; // always >=1
		double dK_dK2 = 2 * K2;
		double X0 =  (rindx[INDX_X0] >=0)?  vector[rindx[INDX_X0]] :  full_vector[INDX_X0] ;
		double Y0 =  (rindx[INDX_Y0] >=0)?  vector[rindx[INDX_Y0]] :  full_vector[INDX_Y0] ;
		for (int y = 0; y < height; y++) {
			double dy = y-Y0;
			double dy2 = dy*dy;
			for (int x = 0; x < width; x++) {
				int indx = y*width+x;
				double dx = x-X0;
				double dx2 = dx*dx;
				double r2 = dx2+dy2+R_OFFS;
				double r = Math.sqrt(r2);
				if (r*RR0 > Math.PI/2) { // outside circle where function is defined
					fx[indx] = C;
					if (jt != null) {
						double df_dC = 1;
						if (rindx[INDX_C] >=0) {
							jt[rindx[INDX_C]][indx] = df_dC;
						}
					}					
				} else {
					double W1 = Math.cos(r*RR0);
					double W2 = Math.cos(r*RR0*K); //Math.cos(r*RR0*(1+K2*K2))
					fx[indx] = A * (W1 * W1 * W2) + C;
					if (jt != null) {
						double dr2_dX0 = -2 * dx;
						double dr2_dY0 = -2 * dy;
						double dr_dr2 = 0.5 / r; // r !=0
						double sin_rRR0 = Math.sin(r * RR0);
						double dW1_dRR0 = -r * sin_rRR0;     // 
						double dW1_dr = -RR0 *sin_rRR0;
						double sin_rKRR0 = Math.sin(r * RR0 * K);
						double dW2_dRR0 =  -K * r * sin_rKRR0;
//						double dW2_dK =    -r * RR0 * sin_rKRR0;
						double dW2_dK2 =  (-r * RR0 * sin_rKRR0) * dK_dK2;  
						double dW2_dr =    -K * RR0 * sin_rKRR0; 
						double df_dA = W1*W1*W2; // +
						double df_dC = 1;        // +  
						double df_dW1 = 2 * A* W1 * W2;
						double df_dW2 = A * W1 * W1;
//						double df_dK =  df_dW2 * dW2_dK; 
						double df_dK2 = df_dW2 * dW2_dK2; 
						double df_dRR0 = df_dW1*dW1_dRR0+df_dW2*dW2_dRR0;
						double df_dr = df_dW1 * dW1_dr + df_dW2 * dW2_dr;  
						double df_dr2 = df_dr * dr_dr2;
						double df_dX0 = df_dr2 * dr2_dX0;
						double df_dY0 = df_dr2 * dr2_dY0;
//						double [] df_dfp = {df_dA,df_dC,df_dRR0, df_dK, df_dX0, df_dY0};
						double [] df_dfp = {df_dA,df_dC,df_dRR0, df_dK2, df_dX0, df_dY0};
						for (int i = 0; i < vector.length; i++) {
							jt[i][indx] = df_dfp[pindx[i]]; 
						}
					}
				}
			}
		}
		return fx;
	}
	private double [] getFxDerivs_orig(
			final double []   vector,
			final double [][] jt, // should be null or initialized with [vector.length][]
			final int         debug_level) {
		int height = y_vector.length/width;
		final double [] fx = new double [y_vector.length];
		if (jt != null) {
			for (int i = 0; i < jt.length; i++) {
				jt[i] = new double [y_vector.length]; // weights.length];
			}
		}
		double A=    (rindx[INDX_A] >=0)?   vector[rindx[INDX_A]] :   full_vector[INDX_A] ; 
		double C=    (rindx[INDX_C] >=0)?   vector[rindx[INDX_C]] :   full_vector[INDX_C] ;
		double RR0 = (rindx[INDX_RR0] >=0)? vector[rindx[INDX_RR0]] : full_vector[INDX_RR0] ;
		double K =   (rindx[INDX_K] >=0)?   vector[rindx[INDX_K]] :   full_vector[INDX_K] ; // >1.0
		double X0 =  (rindx[INDX_X0] >=0)?  vector[rindx[INDX_X0]] :  full_vector[INDX_X0] ;
		double Y0 =  (rindx[INDX_Y0] >=0)?  vector[rindx[INDX_Y0]] :  full_vector[INDX_Y0] ;
		for (int y = 0; y < height; y++) {
			double dy = y-Y0;
			double dy2 = dy*dy;
			for (int x = 0; x < width; x++) {
				int indx = y*width+x;
				double dx = x-X0;
				double dx2 = dx*dx;
				double r2 = dx2+dy2+R_OFFS;
				double r = Math.sqrt(r2);
				if (r*RR0 > Math.PI/2) { // outside circle where function is defined
					fx[indx] = C;
					if (jt != null) {
						double df_dC = 1;
						if (rindx[INDX_C] >=0) {
							jt[rindx[INDX_C]][indx] = df_dC;
						}
					}					
				} else {
					double W1 = Math.cos(r*RR0);
					double W2 = Math.cos(r*RR0*K);
					fx[indx] = A * (W1 * W1 * W2) + C;
					if (jt != null) {
						double dr2_dX0 = -2 * dx;
						double dr2_dY0 = -2 * dy;
						double dr_dr2 = 0.5 / r; // r !=0
						double sin_rRR0 = Math.sin(r * RR0);
						double dW1_dRR0 = -r * sin_rRR0;     // 
						double dW1_dr = -RR0 *sin_rRR0;
						double sin_rKRR0 = Math.sin(r * RR0 * K);
						double dW2_dRR0 =  -K * r * sin_rKRR0;
						double dW2_dK =    -r * RR0 * sin_rKRR0;
						double dW2_dr =    -K * RR0 * sin_rKRR0; 
						double df_dA = W1*W1*W2; // +
						double df_dC = 1;        // +  
						double df_dW1 = 2 * A* W1 * W2;
						double df_dW2 = A * W1 * W1;
						double df_dK =  df_dW2 * dW2_dK; 
						double df_dRR0 = df_dW1*dW1_dRR0+df_dW2*dW2_dRR0;
						double df_dr = df_dW1 * dW1_dr + df_dW2 * dW2_dr;  
						double df_dr2 = df_dr * dr_dr2;
						double df_dX0 = df_dr2 * dr2_dX0;
						double df_dY0 = df_dr2 * dr2_dY0;
						double [] df_dfp = {df_dA,df_dC,df_dRR0, df_dK, df_dX0, df_dY0};
						for (int i = 0; i < vector.length; i++) {
							jt[i][indx] = df_dfp[pindx[i]]; 
						}
					}
				}
			}
		}
		return fx;
	}

	
	

}
