package com.elphel.imagej.cuas;

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

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

import Jama.Matrix;

public class CuasCenterLma {
	public static final String[] PARAMETER_NAMES =  {
			"AZIMUTH_CENTER",
			"TILT_CENTER",
			"AZIMUTH_RADIUS",
			"TILT_RADIUS",
			"AZIMUTH_OMEGA",
			"TILT_OMEGA",
			"ROLL_OMEGA",
			"PHASE" // Azimuth -cos(), Tilt - sin()
	};
	public static final int PARAM_AZIMUTH_CENTER = 0;
	public static final int PARAM_TILT_CENTER =    1;
	public static final int PARAM_AZIMUTH_RADIUS = 2;
	public static final int PARAM_TILT_RADIUS =    3;
	public static final int PARAM_AZIMUTH_OMEGA =  4;
	public static final int PARAM_TILT_OMEGA =     5;
//	public static final int PARAM_ROLL_OMEGA =     6;
	public static final int PARAM_PHASE =          6;
	
	private int          earliest_scene = -1;
	private int          last_scene = -1;
	private boolean []   param_select;
	private double  []   y_vector =        null; // sum of fx(initial parameters) and correlation offsets
	private double  []   weights =         null; //
	private int          num_glob_pars =   0; // number of global parameters (of 4) to be adjusted
//	private int       num_scenes_used = 0; // number of valid scenes for adjustment
	private int     []   scene_indices =   null; // indices of used scenes (in the range, non-null and non-Nan)
	private double  [][] scenes_atr =      null;
	
	private int     []   sel_par_index =   null;
	private int     []   sel_par_rindex =  null;
	private double  []   parameters_vector = null;
	private double  []   full_parameters_vector = null;
	private double       roll_average =    0;
	private double       roll_omega =    0;
	private double []    last_rms =        null; // {rms, rms_pure}, matching this.vector
	private double []    good_or_bad_rms = null; // just for diagnostics, to read last (failed) rms
	private double []    initial_rms =     null; // {rms, rms_pure}, first-calcualted rms
	private double []    last_ymfx =       null;	
	private double [][]  last_jt =         null;
	private double       index_avg =          0; // average scene index

	
	/**
	 * Find center for adjusted scenes, not just the IMS, so omegas should be zero/low
	 * @param quadCLTs
	 * @param ref_scene
	 * @param range
	 * @param debugLevel
	 * @return
	 */
	public static double [][] getCenterATR(
			QuadCLT [] quadCLTs,
			QuadCLT    ref_scene,
			int []     range,
			boolean    disable_AT_omegas,
			int        debugLevel) {
		boolean [] param_select = new boolean[PARAMETER_NAMES.length];
		Arrays.fill(param_select, true);
		if (disable_AT_omegas) {
			param_select[PARAM_AZIMUTH_OMEGA] = false;
			param_select[PARAM_TILT_OMEGA] = false;
		}
		CuasCenterLma cuasCenterLma = new CuasCenterLma(
				param_select, // boolean [] param_select,
				quadCLTs,     // QuadCLT [] quadCLTs,
				ref_scene,    // QuadCLT    ref_scene,
				range,        // int []     range,
				debugLevel);  // int debugLevel)
		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 lmaResult = cuasCenterLma.runLma(
				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
				debugLevel);      // int    debug_level)
		double [][] rslt = {
				cuasCenterLma.getCenter(),
				cuasCenterLma.getRadius(),
				cuasCenterLma.getOmegas(),
				new double [] {cuasCenterLma.getIndexAvg()}};
		if (debugLevel > -3) {
			System.out.println("lmaResult =" +lmaResult+" iterations, RMSE ="+
					cuasCenterLma.getRMS()+" ("+cuasCenterLma.getInitialRMS()+")");
			System.out.println("azimuth_center = "+ rslt[0][0]);
			System.out.println("   tilt_center = "+ rslt[0][1]);
			System.out.println("  average roll = "+ rslt[0][2]);
			System.out.println("azimuth_radius = "+ rslt[1][0]);
			System.out.println("   tilt_radius = "+ rslt[1][1]);
			System.out.println("Using adjusted scenes, omegas should be low!");
			System.out.println(" azimuth_omega = "+ rslt[2][0]);
			System.out.println("    tilt_omega = "+ rslt[2][1]);
			System.out.println("    roll_omega = "+ rslt[2][2]);
			System.out.println("     index_avg = "+ rslt[3][0]);
		}
		return rslt;
	}
	
	/**
	 * Calculate center and radius of view angle rotation from the IMU data, MODIFY data to compensate drifts
	 * @param scenes_atr array of the ATR triplets (calculated from the IMS), some may be nulls. Will be modified!
	 * @param range  {first, last} scene index or null (will use all scenes}    
	 * @param debugLevel debug level
	 * @return array of {{center AT and average roll}, {radius azimuth, radius tilt}, {omegas A,T,R}, {index of zero drift (center)}}
	 * Only first two lines are important, last are used for correction 
	 */
	
	public static double [][] corrCenterATR(
			double [][] scenes_atr,
			int []     range,
			boolean    disable_AT_omegas,
			int        debugLevel) {
		if (range == null) {
			range = new int [] {0, scenes_atr.length -1};
		}
		double [][] center_ATR =  CuasCenterLma.getCenterATR(
				scenes_atr,      // double [][] ims_atr,
				range,           // int []     range,
				disable_AT_omegas, // boolean    disable_AT_omegas,
				debugLevel);     // int        debugLevel)
		double [] omegas = center_ATR[2];
		double index_avg = center_ATR[3][0]; 

		for (int nscene = range[0]; nscene <= range[1]; nscene++) if (scenes_atr[nscene] != null) {
			double time_drift = nscene - index_avg; // zero-average "time" for linear drifting of the azimuth and tilts.
			for (int i = 0; i < omegas.length; i++) {
				scenes_atr[nscene][i] -= time_drift * omegas[i];
			}
		}
		return center_ATR;
	}
	
	/**
	 * Calculate center and radius of view angle rotation from the IMU data
	 * @param scenes_atr array of the ATR triplets (calculated from the IMS), some may be nulls
	 * @param range  {first, last} scene index or null (will use all scenes}    
	 * @param debugLevel debug level
	 * @return array of {{center AT and average roll}, {radius azimuth, radius tilt}, {omegas A,T,R}, {index of zero drift (center)}} 
	 */
	public static double [][] getCenterATR(
			double [][] scenes_atr,
			int []     range,
			boolean    disable_AT_omegas,
			int        debugLevel) {
		boolean [] param_select = new boolean[PARAMETER_NAMES.length];
		Arrays.fill(param_select, true);
		if (disable_AT_omegas) {
			param_select[PARAM_AZIMUTH_OMEGA] = false;
			param_select[PARAM_TILT_OMEGA] = false;
		}
		CuasCenterLma cuasCenterLma = new CuasCenterLma(
				param_select, // boolean [] param_select,
				scenes_atr,     // double [][] scenes_atr,
				range,        // int []     range,
				debugLevel);  // int debugLevel)
		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 lmaResult = cuasCenterLma.runLma(
				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
				debugLevel);      // int    debug_level)
		double [][] rslt = {
				cuasCenterLma.getCenter(),
				cuasCenterLma.getRadius(),
				cuasCenterLma.getOmegas(),
				new double [] {cuasCenterLma.getIndexAvg()}};
		if (debugLevel > -3) {
			System.out.println("lmaResult =" +lmaResult+" iterations, RMSE ="+
					cuasCenterLma.getRMS()+" ("+cuasCenterLma.getInitialRMS()+")");
			System.out.println("azimuth_center = "+ rslt[0][0]);
			System.out.println("   tilt_center = "+ rslt[0][1]);
			System.out.println("  average roll = "+ rslt[0][2]);
			System.out.println("azimuth_radius = "+ rslt[1][0]);
			System.out.println("   tilt_radius = "+ rslt[1][1]);
			System.out.println(" azimuth_omega = "+ rslt[2][0]);
			System.out.println("    tilt_omega = "+ rslt[2][1]);
			System.out.println("    roll_omega = "+ rslt[2][2]);
			System.out.println("     index_avg = "+ rslt[3][0]);
		}
		return rslt;
	}
	
	public CuasCenterLma(
			boolean [] param_select,
			QuadCLT [] quadCLTs,
			QuadCLT    ref_scene,
			int []     range,
			int        debugLevel
			) {
		prepareLMA(
				param_select, // boolean [] param_select,
				quadCLTs,     // QuadCLT [] quadCLTs,
				ref_scene,    //int        ref_index,
				range,        // int []     range,
				debugLevel);  //int debugLevel)
	}

	
	public CuasCenterLma(
			boolean [] param_select,
			double [][] scenes_atr,
			int []     range,
			int        debugLevel) {
		prepareLMA(
				param_select, // boolean [] param_select,
				scenes_atr,     // double [][] scenes_atr,
				range,        // int []     range,
				debugLevel);  //int debugLevel)
	}
	
	/**
	 * Find center and radiuses from the adjusted scenes, so omegas should be zero 
	 * @param param_select
	 * @param quadCLTs
	 * @param ref_scene
	 * @param range
	 * @param debugLevel
	 * @return
	 */
	public int prepareLMA(
			boolean [] param_select,
			QuadCLT [] quadCLTs,
			QuadCLT    ref_scene,
			int []     range,
			int        debugLevel) {
		this.param_select = param_select;
		earliest_scene = range[0];
		last_scene =     range[1];
//        int num_scenes = last_scene- earliest_scene + 1;
		ErsCorrection ers_reference = ref_scene.getErsCorrection();
		double [][] scenes_atr =      new double [quadCLTs.length][];
//		scenes_atr[ref_index] = new double[3]; // all zeros should not be needed, it should be in ers_reference.getSceneATR(ts) for "normal scenes
        for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
			// just checking it is not isolated
        	if (quadCLTs[nscene] == null) {
        		earliest_scene = nscene + 1;
        		break;
        	}
        	// now reference scene should also be in ers_reference.getSceneXYZ(ts)
        	String ts = quadCLTs[nscene].getImageName();
        	scenes_atr[nscene] = ers_reference.getSceneATR(ts);
        }
        return prepareLMA(
        		param_select, // boolean [] param_select,
        		scenes_atr, // double [][] scenes_atr,
    			new int [] {earliest_scene,last_scene}, // int []     range,
    			debugLevel); // int        debugLevel);
	}
	
	public int prepareLMA(
			boolean [] param_select,
			double [][] scenes_atr,
			int []     range,
			int        debugLevel) {
		earliest_scene = range[0];
		last_scene =     range[1];
        int num_scenes = last_scene- earliest_scene + 1;
        for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
			// just checking it is not isolated
        	if (scenes_atr[nscene] == null) {
        		earliest_scene = nscene + 1;
        		break;
        	}
        }
        
		full_parameters_vector = new double[PARAM_PHASE + num_scenes];
		sel_par_rindex = new int[full_parameters_vector.length];
		Arrays.fill(full_parameters_vector, Double.NaN);
		Arrays.fill(sel_par_rindex, -1);

        double [][] at_minmax ={{Double.NaN,Double.NaN},{Double.NaN,Double.NaN}};
//        int num_pars = (PARAMETER_NAMES.length - 1);
        for (int nscene = earliest_scene; nscene <= last_scene; nscene++) if (scenes_atr[nscene] != null){
        	if (!Double.isNaN(scenes_atr[nscene][0]) && !(scenes_atr[nscene][0]>= at_minmax[0][0])){
        		at_minmax[0][0] = scenes_atr[nscene][0];
        	}
        	if (!Double.isNaN(scenes_atr[nscene][1]) && !(scenes_atr[nscene][1]>= at_minmax[1][0])){
        		at_minmax[1][0] = scenes_atr[nscene][1];
        	}
        	if (!Double.isNaN(scenes_atr[nscene][0]) && !(scenes_atr[nscene][0] <= at_minmax[0][1])){
        		at_minmax[0][1] = scenes_atr[nscene][1];
        	}
        	if (!Double.isNaN(scenes_atr[nscene][1]) && !(scenes_atr[nscene][1] <= at_minmax[1][1])){
        		at_minmax[1][1] = scenes_atr[nscene][1];
        	}
//        	num_pars++;
        }
        
        
//		double [] full_parameters_vector = new double[num_pars];
        
        full_parameters_vector[PARAM_AZIMUTH_CENTER] =  0.5 * (at_minmax[0][1] + at_minmax[0][0]);
        full_parameters_vector[PARAM_TILT_CENTER] =     0.5 * (at_minmax[1][1] + at_minmax[1][0]);
        full_parameters_vector[PARAM_AZIMUTH_RADIUS] =  0.5 * (at_minmax[0][1] - at_minmax[0][0]);
        full_parameters_vector[PARAM_TILT_RADIUS] =     0.5 * (at_minmax[1][1] - at_minmax[1][0]);
        full_parameters_vector[PARAM_AZIMUTH_OMEGA] =   0.0;
        full_parameters_vector[PARAM_TILT_OMEGA] =      0.0;
//        full_parameters_vector[PARAM_ROLL_OMEGA] =      0.0;
        if (debugLevel >-3) {
        	System.out.println(at_minmax[0][0]+" <= azimuth <="+at_minmax[0][1]);
        	System.out.println(at_minmax[1][0]+" <=  tilt   <="+at_minmax[1][1]);
        	System.out.println("azimuth_center = "+ full_parameters_vector[0]);
        	System.out.println("   tilt_center = "+ full_parameters_vector[1]);
        	System.out.println("azimuth_radius = "+ full_parameters_vector[2]);
        	System.out.println("   tilt_radius = "+ full_parameters_vector[3]);
        }
//        int par_indx = 4;
        int num_scenes_used = 0;
        for (int nscene = earliest_scene; nscene <= last_scene; nscene++){
        	 if ((scenes_atr[nscene] != null) && !Double.isNaN(scenes_atr[nscene][0]) && !Double.isNaN(scenes_atr[nscene][1])) {
        		 double x = (scenes_atr[nscene][0] - full_parameters_vector[0])/full_parameters_vector[2];
        		 double y = (scenes_atr[nscene][1] - full_parameters_vector[1])/full_parameters_vector[3];
            	 double phase = Math.atan2(y,x);
            	 int par_indx = PARAM_PHASE +  (nscene - earliest_scene);                           
            	 full_parameters_vector[par_indx] = phase;
            	 num_scenes_used++;
        	 }
        }
        scene_indices = new int[num_scenes_used];
        index_avg = 0.0;
        int scene_index = 0;
        for (int nscene = earliest_scene; nscene <= last_scene; nscene++){
        	int par_indx = PARAM_PHASE +  (nscene - earliest_scene);                           
        	if (!Double.isNaN(full_parameters_vector[par_indx])){
        		scene_indices[scene_index++] = nscene;
        		index_avg += nscene;
        	}
        }
        index_avg /= num_scenes_used;
        int num_pars = 0;
        //sel_par_rindex
        for (int i = 0;  i < PARAM_PHASE; i++) {
        	if (param_select[i]) {
        		sel_par_rindex[i] = num_pars++;
        	}
        }
        num_glob_pars = num_pars;
        if (param_select[PARAM_PHASE]) {
        	for (int nscene = earliest_scene; nscene <= last_scene; nscene++){
        		if ((scenes_atr[nscene] != null) && !Double.isNaN(scenes_atr[nscene][0]) && !Double.isNaN(scenes_atr[nscene][1])) {
        			sel_par_rindex[PARAM_PHASE + nscene - earliest_scene] = num_pars++;
        		}
        	}
        }
        sel_par_index = new int[num_pars];
        for (int i = 0; i < sel_par_rindex.length; i++) if (sel_par_rindex[i]>=0){
        	sel_par_index[sel_par_rindex[i]] = i;
        }
        // create Y_vector
        y_vector = new double [2*scene_indices.length];
        for (int i = 0; i < scene_indices.length; i++) {
        	int nscene = scene_indices[i];
        	y_vector[2*i + 0] = scenes_atr[nscene][0];
        	y_vector[2*i + 1] = scenes_atr[nscene][1];
        }
        weights =  new double [y_vector.length];
        Arrays.fill(weights, 1.0/weights.length);
        parameters_vector = new double [sel_par_index.length]; // try not to use it - use instead full_parameters_vector[sel_par_index[i]
        for (int i = 0; i < sel_par_index.length; i++) {
        	parameters_vector[i] = full_parameters_vector[sel_par_index[i]];
        }
        
   		last_jt = new double [parameters_vector.length][];
		if (debugLevel > 1) {
			System.out.println("prepareLMA() 1");
		}
		double [] fx = getFxDerivs(
				parameters_vector, // 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 = this.last_rms.clone();
        // calculate average roll

		double sx=0,sx2=0,sy=0,sxy=0, s0=0;
        for (int i = 0; i < scene_indices.length; i++) {
        	int nscene = scene_indices[i];
        	double time_drift = nscene - index_avg; // zero-average "time" for linear drifting of the azimuth and tilts.         	
        	double roll = scenes_atr[nscene][2];
        	if (!Double.isNaN(roll)) {
        		sx +=  time_drift;
        		sx2 += time_drift * time_drift;
        		sy +=  roll;
        		sxy += time_drift * roll;
        		s0+=1;
        	}
        }
		double d =    (s0 * sx2 - sx * sx);
		roll_average= (sx2 * sy - sx * sxy) / d;
		roll_omega=   (sxy * s0 - sy * sx) / d;
		return 0;
	}
	/*
		double cos_sum=0, sin_sum = 0;
        for (int i = 0; i < scene_indices.length; i++) {
        	int nscene = scene_indices[i];
        	double time_drift = nscene - index_avg; // zero-average "time" for linear drifting of the azimuth and tilts.         	
        	double roll = scenes_atr[nscene][2];
        	cos_sum+=Math.cos(roll);
        	sin_sum+=Math.sin(roll);
        }
		roll_average=Math.atan2(sin_sum, cos_sum);
	 */
	
	private void updateFullParameters(){
        for (int i = 0; i < sel_par_index.length; i++) {
        	full_parameters_vector[sel_par_index[i]] = parameters_vector[i];
        }
	}
 
	private double [] getFxDerivs(
			final double []   vector,
			final double [][] jt, // should be null or initialized with [vector.length][]
			final int         debug_level) {
		final double [] fx = new double [y_vector.length];
		if (jt != null) {
			for (int i = 0; i < jt.length; i++) {
				jt[i] = new double [y_vector.length]; // weights.length];
			}
		}
		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
		final AtomicInteger ai = new AtomicInteger(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nP = ai.getAndIncrement(); nP < scene_indices.length; nP = ai.getAndIncrement()) {
						double phase;
						int full_index =PARAM_PHASE + scene_indices[nP] - earliest_scene;
						double time_drift = scene_indices[nP] - index_avg; // zero-average "time" for linear drifting of the azimuth and tilts. 
						int sel_index = sel_par_rindex[full_index];
						if (sel_index >= 0)  {
							phase = vector[sel_index];
						} else {
							phase = full_parameters_vector[full_index];
						}
						double cos_phase = Math.cos(phase);
						double sin_phase = Math.sin(phase);
						int az0_indx = sel_par_rindex[PARAM_AZIMUTH_CENTER];
						double az0 = (az0_indx >= 0) ? vector[az0_indx] : full_parameters_vector[PARAM_AZIMUTH_CENTER];
						int tl0_indx = sel_par_rindex[PARAM_TILT_CENTER];
						double tl0 = (tl0_indx >= 0) ? vector[tl0_indx] : full_parameters_vector[PARAM_TILT_CENTER];
						int az_rad_indx = sel_par_rindex[PARAM_AZIMUTH_RADIUS];
						double az_rad = (az_rad_indx >= 0) ? vector[az_rad_indx] : full_parameters_vector[PARAM_AZIMUTH_RADIUS];
						int tl_rad_indx = sel_par_rindex[PARAM_TILT_RADIUS];
						double tl_rad = (tl_rad_indx >= 0) ? vector[tl_rad_indx] : full_parameters_vector[PARAM_TILT_RADIUS];
						int az_omega_indx = sel_par_rindex[PARAM_AZIMUTH_OMEGA];
						double azimuth_scale = (az_omega_indx >=0) ? vector[az_omega_indx]: full_parameters_vector[PARAM_AZIMUTH_OMEGA]; 
						int tl_omega_indx = sel_par_rindex[PARAM_TILT_OMEGA];
						double tilt_scale =    (tl_omega_indx >=0) ? vector[tl_omega_indx]: full_parameters_vector[PARAM_TILT_OMEGA]; 
						int indx_x = 2*nP + 0; 
						int indx_y = 2*nP + 1; 
						fx[indx_x] =  az0 + az_rad * cos_phase + azimuth_scale * time_drift;
						fx[indx_y] =  tl0 + tl_rad * sin_phase + tilt_scale *    time_drift;
						if (jt != null) {
							if (az0_indx >= 0) {
								jt[az0_indx][indx_x] = 1; // [][indx_y] = 0
							}
							if (tl0_indx >= 0) {
								jt[tl0_indx][indx_y] = 1; // [][indx_x] = 0
							}
							if (az_rad_indx >= 0) {
								jt[az_rad_indx][indx_x] = cos_phase; // [][indx_y] = 0
							}
							if (tl_rad_indx >= 0) {
								jt[tl_rad_indx][indx_y] = sin_phase; // [][indx_x] = 0
							}
							if (az_omega_indx >=0) {
								jt[az_omega_indx][indx_x] = time_drift; // [][indx_x] = 0
							}
							if (tl_omega_indx >=0) {
								jt[tl_omega_indx][indx_y] = time_drift; // [][indx_x] = 0
							}
							
							if (sel_index >= 0) { // adjusting phases
								jt[sel_index][indx_x] = -sin_phase * az_rad;
								jt[sel_index][indx_y] =  cos_phase * tl_rad;
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return fx;
	}

	private double [] getYminusFxWeighted( // problems. at least with eigen?
			final double []   fx,
			final double []   rms_fp // null or [2]
			) {
		final Thread[]      threads =     ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
		final AtomicInteger ai =          new AtomicInteger(0);
		final double []     wymfw =       new double [fx.length];
		final AtomicInteger ati = new AtomicInteger(0);
		final double [] l2_arr = new double [threads.length];
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					public void run() {
						int thread_num = ati.getAndIncrement();
						for (int i = ai.getAndIncrement(); i < fx.length; i = ai.getAndIncrement()) if (weights[i] > 0) {
							double d = y_vector[i] - fx[i];
							double wd = d * weights[i];
							if (Double.isNaN(wd)) {
								System.out.println("getYminusFxWeighted(): weights["+i+"]="+weights[i]+", wd="+wd+
										", y_vector[i]="+y_vector[i]+", fx[i]="+fx[i]);
							}
							l2_arr[thread_num] += d * wd;
							wymfw[i] = wd;
						}
					}
				};
			}		      
			ImageDtt.startAndJoin(threads);
		double s_rms = 0.0;
		for (double l2:l2_arr) {
			s_rms += l2;
		}
		/*
		double rms_pure = Math.sqrt(s_rms/pure_weight);
		for (int i = 0; i < par_indices.length; i++) {
			int indx = i + num_samples;
			double d = y_vector[indx] - fx[indx]; // fx[indx] == vector[i]
			double wd = d * weights[indx];
			s_rms += d * wd;
			wymfw[indx] =   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 [][] getWJtJlambda( // USED in lwir
			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(QuadCLT.THREADS_MAX);
		final AtomicInteger ai = new AtomicInteger(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int 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];
								if (Double.isNaN(d)) {
									System.out.println("CuasCenterLMA.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;
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		return wjtjl;
	}
	
	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,
			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) {
				return -1; // false; // need to check
			}
			if (debug_level > 1) {
				System.out.println("LMA step "+iter+": {"+rslt[0]+","+rslt[1]+"} full RMS= "+good_or_bad_rms[0]+
						" ("+initial_rms[0]+"), pure RMS="+good_or_bad_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
			}
			if (rslt[1]) {
				break;
			}
			if (rslt[0]) { // good
				lambda *= lambda_scale_good;
			} else {
				lambda *= lambda_scale_bad;
				if (lambda > lambda_max) {
					break; // not used in lwir
				}
			}
//			if (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);
//				String [] lines = printOldNew(true); // boolean allvectors)
//				for (String line : lines) {
//					System.out.println(line);
//				}
			}
			System.out.println();
		}
		if (rslt[0]) {
			updateFullParameters();
		}
		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 (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, 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(
//				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;
	}
	public double [] getCenter() {
		return new double[] {full_parameters_vector[PARAM_AZIMUTH_CENTER],full_parameters_vector[PARAM_TILT_CENTER],roll_average};
	}
	public double [] getRadius() {
		return new double[] {full_parameters_vector[PARAM_AZIMUTH_RADIUS], full_parameters_vector[PARAM_TILT_RADIUS]};
	}
	public double [] getOmegas() {
		return new double[] {full_parameters_vector[PARAM_AZIMUTH_OMEGA], full_parameters_vector[PARAM_TILT_OMEGA], roll_omega};
	}
	
	public double getIndexAvg() {
		return index_avg;
	}
	public double getRMS() {
		return last_rms[0];
	}
	public double getInitialRMS() {
		return initial_rms[0];
	}
	
	public double getAverageRoll() {
		return roll_average;
	}

	public double [] getRoll() {
		return new double [] {roll_average, roll_omega};
	}
	
	
}
