/**
 **
 ** XyzQLma.java - LMA solver for 3-D position and disparity refinement
 **
 ** Copyright (C) 2026 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  XyzQLma.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.tileprocessor;

import java.nio.file.Path;
import java.nio.file.Paths;
import java.text.SimpleDateFormat;
import java.util.Arrays;
import java.util.Calendar;
import java.util.HashMap;
import java.util.HashSet;
import java.util.concurrent.atomic.AtomicInteger;

import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;

import com.elphel.imagej.calibration.CalibrationFileManagement;
import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.ims.Imx5;

import Jama.Matrix;

public class XyzQLma {
	/*
	 * Terminology and frame-model notes for this fitter:
	 *
	 * Scene:
	 * A set of simultaneously captured images (currently 16 for LWIR-16), identified
	 * by one timestamp used in file names as seconds_microseconds.
	 *
	 * Scene sequence:
	 * A continuous capture run (currently ~60 Hz, typically ~496-498 scenes). In older
	 * notes this may also be called scene series.
	 *
	 * Segment:
	 * A contiguous subset of a scene sequence built around one reference scene so each
	 * scene in the segment has sufficient overlap with that reference. Segments are used
	 * for robust inter-scene pose estimation and for collecting wide-baseline target
	 * views. Segments may overlap.
	 *
	 * Reference scene:
	 * The scene defining the segment frame. Its pose is normally {x,y,z,a,t,r}=0 and
	 * all other scene poses in the segment are represented relative to it.
	 *
	 * Data sources:
	 * 1) IMS-derived poses (global accuracy good, local/subpixel weaker)
	 * 2) Image-correlation camera poses (deep-subpixel local accuracy)
	 *
	 * This class estimates the small rotational misalignment between IMS and camera
	 * frames. Input arrays are per-segment relative poses:
	 *   ims_xyzatrs[segment][scene] and xyzatrs[segment][scene]
	 * flattened before fitting.
	 *
	 * Why orientation uses Q*R*Q':
	 * Input rotations are differential (relative to each segment reference scene). The
	 * correction quaternion Q must therefore be applied consistently to both ends of the
	 * relative rotation transform, giving conjugation Q*R*Q'. Using only Q*R is for an
	 * absolute-frame rotation model and is incorrect here.
	 *
	 * Cost weighting:
	 * The global LMA cost mixes translation and orientation terms by translation_weight.
	 * Translation contributes to constrain two axes strongly in near-linear flight,
	 * while orientation often contributes stronger observability for the remaining axis.
	 */
//  private final static int  REGLEN =            1; // number of extra (regularization) samples
    
	private int               N =               0;
	private int               samples =         3;
	private int               samples_x =       3;
	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;
	private double []         y_vector =        null;
	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            xyz_weight;  // weight of all xyz, samples (weight of rotations - pure_weight- xyz_weight     
	private double []         last_ymfx =       null;
	private double [][]       last_jt =         null;
	private double []         fx_saved =        null;
	
	public void prepareLMA(
			double [][][] vect_x, // []{{x,y,z},{a,t,r}}
			double [][][] vect_y, // []{{x,y,z},{a,t,r}}
			double []     vect_w, // one per scene
			double        transl_cost,  // 0.0 ... 1.0;
			double        compass_cost, // 0.0 ... 1.0;
			double        reg_w,        // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  
			final int     debug_level) {
		//MODE_XYZQ
		double []     quat0 = {1,0,0,0};
		N = vect_x.length;
		samples =   7; // 3 + quat0.length;
		samples_x = 7;
		pure_weight = 1.0 - reg_w;
		int extra_samples = (reg_w > 0) ? 3 : 0; // regularize q1..q3, do not pull q0
		x_vector = new double [samples_x * N];
		y_vector = new double [samples *   N + extra_samples];
		weights =  new double [samples *   N + extra_samples];
		parameters_vector = new double[] {quat0[1], quat0[2], quat0[3]}; // optimize only q1,q2,q3
		double sw = 0;
		xyz_weight = 0;
		double [] compass_weights = {1.0, 1.0, 1.0, compass_cost}; // separately control weight of the mismatch related to IMS compass axis
		for (int i = 0; i < N; i++) {
			double sample_weight = ((vect_x[i]== null) || (vect_y[i]== null)) ? 0.0:((vect_w != null)?  vect_w[i] : 1.0);
			double tw = sample_weight * transl_cost;
			double rw = sample_weight * (1.0 - transl_cost);
			if ((vect_x[i]== null) || (vect_y[i]== null)) {
				for (int j = 0; j < samples; j++) {
					y_vector[samples * i + j] = 0.0;
					weights [samples * i + j] = 0.0;
				}				
				for (int j = 0; j < samples_x; j++) {
					x_vector[samples_x * i + j] = 0.0;
					y_vector[samples *   i + j] = 0.0;
					weights [samples *   i + j] = 0.0;
				}				
			} else {
				Rotation   rot_x = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
						vect_x[i][1][0], vect_x[i][1][1], vect_x[i][1][2]);
				Rotation   rot_y = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
						vect_y[i][1][0], vect_y[i][1][1], vect_y[i][1][2]);
				// Translation componets
				for (int j = 0; j < 3; j++) {
					x_vector[samples_x * i + j] = vect_x[i][0][j];
					y_vector[samples * i +   j] = vect_y[i][0][j];
					weights[samples * i +    j] = tw;
					sw += tw;
					xyz_weight += tw;
				}
				x_vector[samples_x * i + 3] = rot_x.getQ0();
				x_vector[samples_x * i + 4] = rot_x.getQ1();
				x_vector[samples_x * i + 5] = rot_x.getQ2();
				x_vector[samples_x * i + 6] = rot_x.getQ3();
				//Rotation components
				if (samples < samples_x) { // no Q0 not used now
					y_vector[samples * i + 3] = rot_y.getQ1();
					y_vector[samples * i + 4] = rot_y.getQ2();
					y_vector[samples * i + 5] = rot_y.getQ3();
					for (int j = 0; j < 3; j++) {
						double w = rw * compass_weights[j+1];
						weights[samples * i + 3 + j] = w;
						sw += w;
					}
				} else { // has Q0
					y_vector[samples * i + 3] = rot_y.getQ0();
					y_vector[samples * i + 4] = rot_y.getQ1();
					y_vector[samples * i + 5] = rot_y.getQ2();
					y_vector[samples * i + 6] = rot_y.getQ3();
					for (int j = 0; j < 4; j++) { // 02/14/2026 - trying q0 . Verify derivatives
						double w = rw * compass_weights[j];
						weights[samples * i + 3 + j] = w;
						sw += w;
					}
				}
			}
		}
		double k = (pure_weight)/sw;
		for (int i = 0; i < weights.length; i++) weights[i] *= k;
		xyz_weight *= k;
		if (extra_samples > 0) {
			double w = (1.0 - pure_weight)/extra_samples;
			for (int i = 0; i < extra_samples; i++) {
				weights [samples * N + i] = w;
				y_vector[samples * N + i] = 0.0; // target for q1..q3 is 0
			}
		}
		last_jt = new double [parameters_vector.length][];		
		if (debug_level > 0) {
			debugYfX ( "",   // String pfx,
					y_vector); // double [] data)
			debugYfX ( "PIMU-",   // String pfx,
					x_vector); // double [] data)
		}
		return;
	}

	private void debugYfX (
			String pfx,
			double [] data) {
	}
	
	public double [] getQuaternion() {
		return vectorToQ(parameters_vector);
	}
	
	private double [] getRms4(double [] rms3) { // rms, rms_pure, xyz_rms, atr_rms
		double [] rms4 = new double [4];
		for (int i = 0; i < rms4.length; i++) rms4[i] = Double.NaN;
		System.arraycopy(rms3, 0, rms4, 0, Math.min(rms3.length, 3));
		if ((rms3.length > 2) && (pure_weight > xyz_weight)) {
			rms4[3] = (rms3[1] * pure_weight - rms3[2] * xyz_weight) / (pure_weight - xyz_weight);
		}
		return rms4;
	}
	
	public double [] getLastRms() {
		return getRms4(last_rms);
	}
	
	public double [] getInitialRms() {
		return getRms4(initial_rms);
	}
	
	public double [] getGoodOrBadRms() {
		return getRms4(good_or_bad_rms);
	}
	
	public double [] getLastFx() {
		return getFxDerivs(
				parameters_vector, // double []         vector,
				null,              // final double [][] jt
				-3);               // final int         debug_level
	}
	
	public double [] getLastFx(int debug_level) {
		return getFxDerivs(
				parameters_vector, // double []         vector,
				null,              // final double [][] jt
				debug_level);      // final int         debug_level
	}
	
	public void saveFx(int debug_level) {
		fx_saved = getFxDerivs(
				parameters_vector, // double []         vector,
				null,              // final double [][] jt
				debug_level);      // final int         debug_level
	}
	
	public double [] getSavedFx() {
		return fx_saved;
	}
	
	public double [] getX() {
		return x_vector;
	}
	
	public double [] getY() {
		return y_vector;
	}
	
	public double [] getW() {
		return weights;
	}
	
	private double [] getYminusFxWeighted(
			final double [] fx,
			final double [] rms_fp, // null or [3]
			boolean noNaNs) {
		final double [] wymfw = new double[fx.length];
		double s_rms = 0.0;
		double sxyz_rms = 0.0;
		double rms_pure = Double.NaN;
		double rms_pure_xyz = Double.NaN;
		for (int i = 0; i < fx.length; i++) {
			double d = y_vector[i] - fx[i];
			double wd = d * weights[i];
			if (Double.isNaN(wd)) {
				if (noNaNs) {
					if (rms_fp != null) {
						rms_fp[0] = Double.NaN;
						rms_fp[1] = Double.NaN;
						rms_fp[2] = Double.NaN;
					}
					return null;
				}
				wd = 0.0;
				d = 0.0;
			}
			if (i == (samples * N)) {
				if (pure_weight > 0.0) {
					rms_pure = Math.sqrt(s_rms / pure_weight);
				}
				if (xyz_weight > 0.0) {
					rms_pure_xyz = Math.sqrt(sxyz_rms / xyz_weight);
				}
			}
			wymfw[i] = wd;
			double wd2 = d * wd;
			s_rms += wd2;
			if ((i % samples) < 3) {
				sxyz_rms += wd2;
			}
		}
		double rms = Math.sqrt(s_rms); // assuming sum(weights) == 1.0
		if (Double.isNaN(rms_pure)) rms_pure = rms;
		if (Double.isNaN(rms_pure_xyz)) {
			rms_pure_xyz = (xyz_weight > 0.0) ? Math.sqrt(sxyz_rms / xyz_weight) : Double.NaN;
		}
		if (rms_fp != null) {
			rms_fp[0] = rms;
			rms_fp[1] = rms_pure;
			rms_fp[2] = rms_pure_xyz;
		}
		return wymfw;
	}
	
	private double [][] getWJtJlambda(
			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++) {
					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;
				}
			}
		}
		return wjtjl;
	}
	
	private boolean [] lmaStep(
			double lambda,
			double rms_diff,
			int debug_level) {
		boolean noNaNs = true;
		boolean [] rslt = {false, false};
		if (this.last_rms == null) { // first call
			last_rms = new double[3];
			double [] fx0 = getFxDerivs(
					parameters_vector, // double []         vector,
					last_jt,           // final double [][] jt
					debug_level);      // final int         debug_level
			if (debug_level > 5) { // remove?
				double    delta = 1E-5;
				System.out.println("\n\n");
				double err = compareJT(
						parameters_vector, // double [] vector,
						delta);            // double    delta);
				System.out.println("Maximal error = "+err);
			}
			
			last_ymfx = getYminusFxWeighted(
					fx0,      // final double [] fx
					last_rms, // final double [] rms_fp
					noNaNs);  // boolean noNaNs
			this.initial_rms = this.last_rms.clone();
			this.good_or_bad_rms = this.last_rms.clone();
			if (last_ymfx == null) {
				return null;
			}
		}
		Matrix y_minus_fx_weighted = new Matrix(this.last_ymfx, this.last_ymfx.length);
		Matrix wjtjlambda = new Matrix(getWJtJlambda(
				lambda,       // double lambda
				this.last_jt  // double[][] jt
		));
		if (debug_level > 2) {
			System.out.println("JtJ + lambda*diag(JtJ");
			wjtjlambda.print(18, 6);
		}
		Matrix jtjl_inv;
		try {
			jtjl_inv = wjtjlambda.inverse();
		} catch (RuntimeException e) {
			rslt[1] = true;
			return rslt;
		}
		if (debug_level > 2) {
			System.out.println("(JtJ + lambda*diag(JtJ).inv()");
			jtjl_inv.print(18, 6);
		}
		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 [] delta = mdelta.getColumnPackedCopy();
		double [] new_vector = parameters_vector.clone();
		for (int i = 0; i < parameters_vector.length; i++) {
			new_vector[i] += delta[i];
		}
		new_vector = normalizeQ123(new_vector); // keep q1..q3 valid for q0=sqrt(1-q1^2-q2^2-q3^2)

		double [] fx = getFxDerivs(
				new_vector, // double []         vector,
				last_jt,    // final double [][] jt
				debug_level // final int         debug_level
		);
		double [] rms = new double[3];
		last_ymfx = getYminusFxWeighted(
				fx,       // final double [] fx
				rms,      // final double [] rms_fp
				noNaNs);  // boolean noNaNs
		this.good_or_bad_rms = rms.clone();
		if ((rms[0] < this.last_rms[0]) && (last_ymfx != null)) { // 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();
		} else { // worsened, restore state
			rslt[0] = false;
			rslt[1] = false;
			fx = getFxDerivs(
					parameters_vector, // double []         vector,
					last_jt,           // final double [][] jt
					debug_level);      // final int         debug_level
			last_ymfx = getYminusFxWeighted(
					fx,            // final double [] fx
					this.last_rms, // final double [] rms_fp
					noNaNs);       // boolean noNaNs
			if (last_ymfx == null) {
				return null;
			}
		}
		return rslt;
	}
	
	public int runLma( // <0 failed, >=0 last iteration index
			double lambda,
			double lambda_scale_good,
			double lambda_scale_bad,
			double lambda_max,
			double rms_diff,
			int    num_iter,
			boolean last_run,
			int    debug_level) {
		boolean [] rslt = {false, false};
		this.last_rms = null;
		int iter = 0;
		for (iter = 0; iter < num_iter; iter++) {
			rslt = lmaStep(
					lambda,     // double lambda
					rms_diff,   // double rms_diff
					debug_level // int debug_level
			);
			if (rslt == null) {
				return -1;
			}
			if (debug_level > 1) {
				double [] good_or_bad4 = getGoodOrBadRms();
				double [] initial_rms4 = getInitialRms();
				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] + "), " +
						"XYZ RMS=" + good_or_bad4[2] + " (" + initial_rms4[2] + "), ATR RMS=" + good_or_bad4[3] + " (" + initial_rms4[3] + "), " +
						"lambda=" + lambda);
			}
			if (rslt[1]) {
				break;
			}
			if (rslt[0]) {
				lambda *= lambda_scale_good;
			} else {
				lambda *= lambda_scale_bad;
				if (lambda > lambda_max) {
					break;
				}
			}
		}
		if (!rslt[0]) {
			if ((last_rms != null) && (initial_rms != null) && (last_rms[0] < initial_rms[0])) {
				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");
			}
		} else if (debug_level > 1) {
			if (iter >= num_iter) {
				System.out.println("Step " + iter + ": Improved, but number of steps exceeded maximal");
			} else {
				System.out.println("Step " + iter + ": LMA: Success");
			}
		}
		if (debug_level > 0 && last_rms != null && initial_rms != null) {
			double [] last_rms4 = getLastRms();
			double [] initial_rms4 = getInitialRms();
			System.out.println("LMA: full RMS=" + last_rms[0] + " (" + initial_rms[0] + "), pure RMS=" + last_rms[1] + " (" + initial_rms[1] + "), " +
					"XYZ RMS=" + last_rms4[2] + " (" + initial_rms4[2] + "), ATR RMS=" + last_rms4[3] + " (" + initial_rms4[3] + "), " +
					"lambda=" + lambda);
		}
		return rslt[0] ? iter : -1;
	}

	private double [] getFxDerivs(
			double []         vector,
			final double [][] jt, // should be null or initialized with [vector.length][]
			final int         debug_level) {
		double [] fx = new double [weights.length];
		double [] q4 = vectorToQ(vector);
		if (jt != null) {
			for (int i = 0; i < vector.length; i++) {
				jt[i] = new double [weights.length];
			}
		}
		for (int i = 0; i < N; i++) {
			int bix = samples_x * i;
			int bof = samples   * i;
			double [] xyzQ = new double[] {
					x_vector[bix],
					x_vector[bix + 1],
					x_vector[bix + 2],
					x_vector[bix + 3],
					x_vector[bix + 4],
					x_vector[bix + 5],
					x_vector[bix + 6]
			};
			double [] f7 = new double[7];
			if (jt != null) {
				double [][] jt7 = getJT3(xyzQ, q4);
				applyQCore(xyzQ, q4, f7, null);
				for (int p = 0; p < vector.length; p++) {
					for (int k = 0; k < 7; k++) {
						jt[p][bof + k] = jt7[p][k];
					}
				}
			} else {
				applyQCore(xyzQ, q4, f7, null);
			}
			System.arraycopy(f7, 0, fx, bof, 7);
		}
		// Optional regularization columns at the end: pull q1..q3 to 0.
		int extra = weights.length - samples * N;
		int base = samples * N;
		if (extra >= 3) {
			for (int j = 0; j < 3; j++) {
				int col = base + j;
				if (col < fx.length) {
					fx[col] = vector[j];
					if (jt != null) jt[j][col] = 1.0;
				}
			}
		}
		return fx;
	}
	
	
	/**
	 * Apply unit quaternion Q[4] (rotation around global {0,0,0}) to a body at {x,y,z} in global frame
	 * and orientation of its frame represented by the unit quaternion {q0,q1,q21,q3}
	 * @param xyzQ array {x,y,z,q0,q1,q2,q3}
	 * @param Q array Q[4]
	 * @return {x',y',z',q0',q1',q2',q3'} consisting of new global coordinates x1,x2,x3 and orientation
	 * as unit quaternion {q0',q1',q2',q3'} 
	 */
	private static double [] applyQ(
			double [] xyzQ,
			double [] Q) {
		double [] result = new double[7];
		applyQCore(xyzQ, Q, result, null);
		return result;
	}
	/**
	 * Calculate transposed Jacobian [4][7] for the method applyQ(), assuming normalization (unity length) of Q,
	 * so partial derivative by each component of Q indirectly changes all other components to keep sum of squares equal 1 
	 * @param xyzQ array {x,y,z,q0,q1,q2,q3}
	 * @param Q array Q[4]
	 * @return [4][7] array of partial derivatives with respect to the components of Q
	 */
	private static double [][] getJT(
			double [] xyzQ,
			double [] Q) {
		double [][] jt = new double [4][7];
		applyQCore(xyzQ, Q, null, jt);
		return jt;
	}
	private static double [][] getJTdelta(
			double [] xyzQ,
			double [] Q,
			double    delta) {
		double [][] jt = new double [4][7];
		for (int i = 0; i < 4; i++) {
			double [] qp = Q.clone();
			double [] qm = Q.clone();
			qp[i] += 0.5*delta;
			qm[i] -= 0.5*delta;
			qp = normalizeQ(qp);
			qm = normalizeQ(qm);
			double [] fp = applyQ(xyzQ, qp);
			double [] fm = applyQ(xyzQ, qm);
			for (int j = 0; j < 7; j++) {
				jt[i][j] = (fp[j] - fm[j]) / delta;
			}
		}
		return jt;
	}

	private static double [] normalizeQ(double [] q) {
		double s2 = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
		if (s2 <= 0.0) {
			return new double[] {1.0, 0.0, 0.0, 0.0};
		}
		double k = 1.0 / Math.sqrt(s2);
		return new double[] {k * q[0], k * q[1], k * q[2], k * q[3]};
	}

	private static double [] conjQ(double [] q) {
		return new double[] {q[0], -q[1], -q[2], -q[3]};
	}

	private static double [] mulQ(double [] a, double [] b) {
		double a0 = a[0];
		double a1 = a[1];
		double a2 = a[2];
		double a3 = a[3];
		double b0 = b[0];
		double b1 = b[1];
		double b2 = b[2];
		double b3 = b[3];
		return new double[] {
				a0 * b0 - a1 * b1 - a2 * b2 - a3 * b3,
				a0 * b1 + a1 * b0 + a2 * b3 - a3 * b2,
				a0 * b2 - a1 * b3 + a2 * b0 + a3 * b1,
				a0 * b3 + a1 * b2 - a2 * b1 + a3 * b0
		};
	}

	private static double [] rotateVecByQ(double [] v, double [] q) {
		double [] vq = new double[] {0.0, v[0], v[1], v[2]};
		double [] r = mulQ(mulQ(q, vq), conjQ(q));
		return new double[] {r[1], r[2], r[3]};
	}

	private static double [] normalizeQ123(double [] v3) {
		double [] u = v3.clone();
		double s2 = u[0] * u[0] + u[1] * u[1] + u[2] * u[2];
		if (s2 >= 1.0) {
			double k = (1.0 - 1.0e-12) / Math.sqrt(s2);
			u[0] *= k;
			u[1] *= k;
			u[2] *= k;
		}
		return u;
	}

	private static double [] vectorToQ(double [] v3) {
		double [] u = normalizeQ123(v3);
		double s2 = u[0] * u[0] + u[1] * u[1] + u[2] * u[2];
		double q0 = Math.sqrt(Math.max(0.0, 1.0 - s2));
		return new double[] {q0, u[0], u[1], u[2]};
	}

	private static double [][] getJT3(
			double [] xyzQ,
			double [] q4) {
		double [][] jt3 = new double[3][7];
		double [][] dfdq = getDfdqNormalizedQ(xyzQ, q4);
		double q0 = q4[0];
		double q1 = q4[1];
		double q2 = q4[2];
		double q3 = q4[3];

		double[] u = new double[] {q1, q2, q3};
		for (int i = 0; i < 3; i++) {
			double dq0du = (q0 > 1.0e-12) ? (-u[i] / q0) : 0.0;
			for (int k = 0; k < 7; k++) {
				jt3[i][k] = dfdq[k][i + 1] + dfdq[k][0] * dq0du;
			}
		}
		return jt3;
	}
	
	private double compareJT(
			double [] vector,
			double    delta) {
		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, // double []         vector,
				jt,     // final double [][] jt
				1);     // final int         debug_level
		double [][] jt_delta = getFxDerivsDelta(
				vector, // double []         vector,
				delta,  // final double      delta
				-1);    // final int         debug_level
		for (int n = 0; 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++) {
				double e = jt[i][n] - jt_delta[i][n];
				System.out.print(String.format("\t%12.9f", e));
				errors[i] = Math.max(errors[i], Math.abs(e));
			}
			System.out.println();
		}
		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();
		double err = 0.0;
		for (int i = 0; i < vector.length; i++) {
			err = Math.max(err, errors[i]);
		}
		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 [] vp = vector.clone();
			double [] vm = vector.clone();
			vp[nv] += 0.5 * delta;
			vm[nv] -= 0.5 * delta;
			double [] fp = getFxDerivs(
					vp,   // double []         vector
					null, // final double [][] jt
					debug_level);
			double [] fm = getFxDerivs(
					vm,   // double []         vector
					null, // final double [][] jt
					debug_level);
			for (int i = 0; i < weights.length; i++) {
				jt[nv][i] = (fp[i] - fm[i]) / delta;
			}
		}
		return jt;
	}
	
	private static void applyQCore(
			double [] xyzQ,
			double [] Q,
			double [] out,      // nullable, [7]
			double [][] jt) {   // nullable, [4][7]
		double q0 = Q[0];
		double q1 = Q[1];
		double q2 = Q[2];
		double q3 = Q[3];
		double x = xyzQ[0];
		double y = xyzQ[1];
		double z = xyzQ[2];
		double r0 = xyzQ[3];
		double r1 = xyzQ[4];
		double r2 = xyzQ[5];
		double r3 = xyzQ[6];

		if (out != null) {
			out[0] = (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * x + 2.0 * (q1 * q2 - q0 * q3) * y + 2.0 * (q1 * q3 + q0 * q2) * z;
			out[1] = 2.0 * (q1 * q2 + q0 * q3) * x + (q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3) * y + 2.0 * (q2 * q3 - q0 * q1) * z;
			out[2] = 2.0 * (q1 * q3 - q0 * q2) * x + 2.0 * (q2 * q3 + q0 * q1) * y + (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * z;
			double [] qr = mulQ(new double[] {q0, q1, q2, q3}, new double[] {r0, r1, r2, r3});
			double [] qc = new double[] {q0, -q1, -q2, -q3};
			double [] qrq = mulQ(qr, qc);
			out[3] = qrq[0];
			out[4] = qrq[1];
			out[5] = qrq[2];
			out[6] = qrq[3];
		}
		if (jt == null) {
			return;
		}
		// Derivatives of outputs by normalized quaternion components q0..q3
		double [][] dfdq = getDfdqNormalizedQ(xyzQ, new double[] {q0, q1, q2, q3});

		// Chain rule: d/dQ_i = sum_j (d/dq_j) * (dq_j/dQ_i), with q = Q/|Q|
		double s2 = Q[0] * Q[0] + Q[1] * Q[1] + Q[2] * Q[2] + Q[3] * Q[3];
		if (s2 <= 0.0) {
			for (int i = 0; i < 4; i++) {
				for (int k = 0; k < 7; k++) {
					jt[i][k] = 0.0;
				}
			}
			return;
		}
		double s = Math.sqrt(s2);
		double invs = 1.0 / s;
		double [] qn = new double[] {Q[0] * invs, Q[1] * invs, Q[2] * invs, Q[3] * invs};
		for (int i = 0; i < 4; i++) {
			for (int outi = 0; outi < 7; outi++) {
				double v = 0.0;
				for (int j = 0; j < 4; j++) {
					double dqjdQi = ((j == i) ? invs : 0.0) - (qn[j] * qn[i]) * invs;
					v += dfdq[outi][j] * dqjdQi;
				}
				jt[i][outi] = v;
			}
		}
	}
	
	private static double [][] getDfdqNormalizedQ(
			double [] xyzQ,
			double [] q) {
		double q0 = q[0];
		double q1 = q[1];
		double q2 = q[2];
		double q3 = q[3];
		double x = xyzQ[0];
		double y = xyzQ[1];
		double z = xyzQ[2];
		double r0 = xyzQ[3];
		double r1 = xyzQ[4];
		double r2 = xyzQ[5];
		double r3 = xyzQ[6];
		double [][] dfdq = new double[7][4];
		// translation part
		dfdq[0][0] =  2.0 * ( q0 * x - q3 * y + q2 * z);
		dfdq[0][1] =  2.0 * ( q1 * x + q2 * y + q3 * z);
		dfdq[0][2] =  2.0 * (-q2 * x + q1 * y + q0 * z);
		dfdq[0][3] =  2.0 * (-q3 * x - q0 * y + q1 * z);

		dfdq[1][0] =  2.0 * ( q3 * x + q0 * y - q1 * z);
		dfdq[1][1] =  2.0 * ( q2 * x - q1 * y - q0 * z);
		dfdq[1][2] =  2.0 * ( q1 * x + q2 * y + q3 * z);
		dfdq[1][3] =  2.0 * ( q0 * x - q3 * y + q2 * z);

		dfdq[2][0] =  2.0 * (-q2 * x + q1 * y + q0 * z);
		dfdq[2][1] =  2.0 * ( q3 * x + q0 * y - q1 * z);
		dfdq[2][2] =  2.0 * (-q0 * x + q3 * y - q2 * z);
		dfdq[2][3] =  2.0 * ( q1 * x + q2 * y + q3 * z);

		double [] qq = new double[] {q0, q1, q2, q3};
		double [] rr = new double[] {r0, r1, r2, r3};
		double [] qc = new double[] {q0, -q1, -q2, -q3};
		double [] qr = mulQ(qq, rr);
		double [][] e = {
				{1.0, 0.0, 0.0, 0.0},
				{0.0, 1.0, 0.0, 0.0},
				{0.0, 0.0, 1.0, 0.0},
				{0.0, 0.0, 0.0, 1.0}};
		double [][] ec = {
				{1.0, 0.0, 0.0, 0.0},
				{0.0,-1.0, 0.0, 0.0},
				{0.0, 0.0,-1.0, 0.0},
				{0.0, 0.0, 0.0,-1.0}};
		for (int i = 0; i < 4; i++) {
			double [] term1 = mulQ(mulQ(e[i], rr), qc);
			double [] term2 = mulQ(qr, ec[i]);
			dfdq[3][i] = term1[0] + term2[0];
			dfdq[4][i] = term1[1] + term2[1];
			dfdq[5][i] = term1[2] + term2[2];
			dfdq[6][i] = term1[3] + term2[3];
		}
		return dfdq;
	}

	/**
	 * Get average length ratio (X,Y,Z are not offest)
	 * @param vect_x first array of poses: [nscene]{{x,y,z},{a,t,r}} 
	 * @param vect_y second array of poses: [nscene]{{x,y,z},{a,t,r}}
	 * @param first_last null (use all data) or a pair of first, last (inclusive) indices
	 * @return ratio of average length of the XYZ of the vect_y to vect_x
	 */
	public static double getLengthRatio(
			double [][][] vect_x, // []{{x,y,z},{a,t,r}}
			double [][][] vect_y, //  []{{x,y,z},{a,t,r}}
			int []        first_last){
		if (first_last == null) {
			first_last = new int [] {0,vect_x.length-1}; 
		}
		double [][][] scaled_xyz = new double [vect_y.length][][];
		double sx2=0, sy2=0;
		for (int i = first_last[0]; i<= first_last[1]; i++) if ((vect_x[i] != null) && (vect_y[i] != null)){
			double lx2=0, ly2=0;
			for (int j=0; j < 3; j++) {
				double x=vect_x[i][0][j];
				double y=vect_y[i][0][j];
				lx2 += x*x;
				ly2 += y*y;
			}
			if (!Double.isNaN(lx2) && !Double.isNaN(ly2)) {
				sx2 += lx2;
				sy2 += ly2;
			}
		}
		return Math.sqrt(sy2/sx2);
	}

	/**
	 * Scale translation components of the pose vectors array
	 * @param s scaler to apply (when used with getLengthRatio(), s = 1.0/getLengthRatio() ) 
	 * @param vect_y array of poses: [nscene]{{x,y,z},{a,t,r}}
	 * @param first_last null (use all data) or a pair of first, last (inclusive) indices
	 * @return scaled (only x,y,z) of the vect_y
	 */
	
	public static double [][][] scaleXYZ(
			double        s,
			double [][][] vect_y, // []{{x,y,z},{a,t,r}}
			int []        first_last){
		if (first_last == null) {
			first_last = new int [] {0,vect_y.length-1}; 
		}
		double [][][] scaled_xyz = new double [vect_y.length][][];
		for (int i = first_last[0]; i<= first_last[1]; i++) if (vect_y[i] != null){
			scaled_xyz[i] = new double [][] {
				{s* vect_y[i][0][0],s* vect_y[i][0][1],s* vect_y[i][0][2]},
				vect_y[i][1]};
		}
		return scaled_xyz;
	}
	
	public double [] getMinMaxDiag(
			int debug_level){
		double [][] jt = new double [parameters_vector.length][];    
		//double [] fx = 
		getFxDerivs(
				parameters_vector, // double []         vector,
				jt,                // final double [][] jt, // should be null or initialized with [vector.length][]
				debug_level);      // final int         debug_level)
		Matrix wjtjlambda = new Matrix(getWJtJlambda(
				0, // *10, // temporary
				jt)); // double [][] jt)
		double [] mn_mx= {Double.NaN,Double.NaN};
		int si = (wjtjlambda.get(0,0) == 0)? 1 : 0;
		for (int i = si; i < parameters_vector.length; i++) {
			double d = wjtjlambda.get(i,i);
			if (!(d > mn_mx[0])) mn_mx[0] = d;
			if (!(d < mn_mx[1])) mn_mx[1] = d;

		}
		return mn_mx;
	}

	String getLmaTable(
			boolean use_atr,
			String sep,
			int    debugLevel) {
		int []  quat_pointers = {3,4};
		String [] titles = {"X","Y","Z","Q0","Q1","Q2","Q3"};
		boolean q4 = quat_pointers[1] == 4;
		StringBuffer sb = new StringBuffer();
		double [] y =      getY();
		double [] fx0 =    getSavedFx();
		double [] fx =     getLastFx(debugLevel);
		boolean   has_fx0= (fx0 != null);
		String [] groups = {"y-", "fX0-", "fX-"};
		for (int n = 0; n < 3; n++) if ((n != 1 ) || has_fx0) {
			boolean last_group = (n == 2);
			for (int i = 0; i < titles.length; i++) {
				sb.append(groups[n]+titles[i]);
				if (!last_group || (i != (titles.length - 1))) {
					sb.append(sep);
				} else {
					sb.append("\n");
				}
			}
		}
		double [][] data = {y,fx0,fx};
		for (int nscene = 0; nscene < N; nscene++) {
			for (int n = 0; n < 3; n++) if ((n != 1 ) || has_fx0) {
				boolean last_group = (n == 2);
				double [] line = new double [titles.length];
				System.arraycopy(data[n], samples * nscene, line, 0,  samples);
				if (line.length > samples) { // add atr
					double [] quat = new double [4]; 
					System.arraycopy(line, quat_pointers[0], quat, q4 ? 0 : 1,  quat_pointers[1]);
					if (!q4) { // add q0 > 0;
						quat[0] = Math.sqrt(1.0 -quat[1]*quat[1] -quat[2]*quat[2] -quat[3]*quat[3]);
					}
					Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false);
					double [] atr = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
					System.arraycopy(atr, 0, line, samples,  atr.length);
					
				}
				for (int i = 0; i < titles.length; i++) {
					sb.append(line[i]);
					if (!last_group || (i != (titles.length - 1))) {
						sb.append(sep);
					} else {
						sb.append("\n");
					}
				}				
			}			
		}
		return sb.toString();
	}
// moved here from SeriesInfinityCorrection	
    public static double[] adjustImuOrient(  // multi-segment
    		CLTParameters clt_parameters,     // CLTParameters clt_parameters,
    		QuadCLT[][]   quadCLTss,
    		int []        ref_indices,
    		QuadCLT       index_scene, // where to put result
    		boolean       save_details,
    		int           debugLevel) {
//    	@Deprecated
//		boolean orient_by_move =    clt_parameters.imp.orient_by_move; // use translation data to adjust IMU orientation
//    	@Deprecated
//		boolean orient_by_rot =     clt_parameters.imp.orient_by_rot; // use rotation data to adjust IMU orientation
//		if (!orient_by_move && !orient_by_rot) {
//			System.out.println("Neither translation nor rotation data are enabled to adjust IMU orientation relative to the camera");
//			return null;
//		}
//		boolean orient_combo =     clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
		boolean apply_imu_orient =  clt_parameters.imp.apply_imu_orient; // apply  IMU misalignment to the camera if adjusted
		boolean adjust_gyro =       clt_parameters.imp.adjust_gyro; // adjust qyro omegas offsets
		boolean apply_gyro =        clt_parameters.imp.apply_gyro; // apply adjusted qyro omegas offsets
		boolean adjust_accl =       clt_parameters.imp.adjust_accl; // adjust IMU velocities scales
		boolean apply_accl =        clt_parameters.imp.apply_accl; // apply IMU velocities scales
		double  quat_max_change =   clt_parameters.imp.quat_max_change; // do not apply if any component of the result exceeds
		double  quat_min_lin =      clt_parameters.imp.quat_min_lin; // meters, minimal distance per axis to adjust IMS velocity scale
//		boolean use3=               clt_parameters.imp.use3; // false; // false - full4 quaternion (with scale), true - only q1,q2,q3
		double  transl_cost =       clt_parameters.imp.wser_orient_transl; // AGL corrected, 0.5 - equal weights, 0.0 - rotation only, 1.0 - translation-only
		double  compass_cost =      clt_parameters.imp.wser_orient_compass; // weight of IMS compass angle relative to others (1.0 - all equal)
		
		int num_refs = ref_indices.length;
		double [][][][] pimu_xyzatrs = new double[num_refs][][][];
		double [][][][] xyzatrs =      new double[num_refs][][][];
		ErsCorrection [] ers_refs =    new ErsCorrection[num_refs];
		// calculate average Z for the sequence (does not need to be very accurate
		double sum_avgz_w = 0,sum_avgz=0; 
		for (int nref = 0; nref < num_refs; nref++) {
			int last_index = quadCLTss[nref].length-1;
			QuadCLT[] quadCLTs = quadCLTss[nref];
			int ref_index  = ref_indices[nref];
			ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();
			pimu_xyzatrs[nref] = QuadCLT.integratePIMU(
					clt_parameters,          // final CLTParameters clt_parameters,
					quadCLTs,                // final QuadCLT[]     quadCLTs,
					null, // quat_corr,      // double []  quat_corr,
					ref_index,               // final int           ref_index,
					null,                    // double [][][]       dxyzatr,
					0, // earliest_scene,    // final int           early_index,
					last_index,              //  last_index,     //(quadCLTs.length -1) // int           last_index,
					debugLevel);               // final int           debugLevel
			xyzatrs[nref] =  new double [quadCLTss[nref].length][][];
			for (int nscene = 0; nscene <= last_index; nscene++) {
				String ts = quadCLTs[nscene].getImageName();
				xyzatrs[nref][nscene] = ers_ref.getSceneXYZATR(ts);
			}
			ers_refs[nref] = ers_ref;
			double w = 1.0;
			sum_avgz_w += w;
			sum_avgz += w * quadCLTs[ref_index].getAverageZ(true); // in meters
		}
		double avg_z = sum_avgz/sum_avgz_w; 
    	double [] rms = new double[7];
    	double [] quat = new double[4];
    	int debug_lev = debugLevel; // 3;
    	if (debugLevel > -3) {
    		System.out.println("adjustImuOrient(): transl_cost="+transl_cost+", compass_cost="+compass_cost);
    	}
//    	String        suffix = save_details ? (orient_combo ? QuadCLTCPU.IMU_CALIB_COMBO_SUFFIX: QuadCLTCPU.IMU_CALIB_DETAILS_SUFFIX) : null;
    	String        suffix = save_details ? QuadCLTCPU.IMU_CALIB_DETAILS_SUFFIX : null;
    	double [] y_scale_p = new double[1];
    	double [][][][] rotated_xyzatrs = rotateImsToCameraXYZ(
    			clt_parameters,     // CLTParameters clt_parameters,
    			avg_z,              // double        avg_height,
    			transl_cost,        // translation_weight, // double        translation_weight,
    			compass_cost,       // double        compass_cost, // 0.05;  // weight of IMS compass angle relative to others (1.0 - all equal)
    			quadCLTss,          // QuadCLT[][]     quadCLTss,
    			xyzatrs,            // double [][][][] xyzatrs,
    			pimu_xyzatrs,       // double [][][][] ims_xyzatrs,
    			ref_indices,        // int []          ref_indices,
    			rms,                // double []     rms, // null or double[5];
    			quat,               // double []     quaternion, // null or double[4]
    			y_scale_p,          // double [] y_scale_p
    			index_scene,        // QuadCLT       index_scene, // where to put result
    			suffix,             // String        suffix,
    			debug_lev);         // int           debugLevel
    	if (rotated_xyzatrs != null) {
        	Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
        	double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
        	double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(
        			ims_mount_atr, // double [] ims_atr,
        			quat); // double [] corr_q)
        	StringBuffer sb = new StringBuffer();
        	sb.append(new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(Calendar.getInstance().getTime())+"\n");
    		sb.append("Applying correction to the IMS mount orientation:\n");
        	sb.append("avg_z= "+avg_z+" m\n");
        	sb.append("transl_cost=   "+transl_cost+"\n");
        	sb.append("compass_cost=  "+compass_cost+"\n");
        	sb.append("y_scale=       "+y_scale_p[0]+"\n");
        	sb.append("RMSe=");
        	for (int i = 0; i < rms.length-1; i++) sb.append(rms[i]+",");
        	sb.append(rms[rms.length-1]+"\n");
 			double []  corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
			double []  corr_degrees = new double[3];
			double quat_scale = Math.sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]);
			for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
			double [] new_degrees = new double[3];
			for (int i = 0; i < 3; i++) new_degrees[i]=new_ims_mount_atr[i]*180/Math.PI;
			sb.append("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
			sb.append("scale="+quat_scale+"\n");
			double angle = Math.sqrt(corr_angles[0]*corr_angles[0] + corr_angles[1]*corr_angles[1] + corr_angles[2]*corr_angles[2]);
			sb.append("delta ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"] ("+ angle+")\n");
			sb.append("delta ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"] ("+ angle*180/Math.PI+")\n");
			sb.append("  new ATR(rad)=["+new_ims_mount_atr[0]+", "+new_ims_mount_atr[1]+", "+new_ims_mount_atr[2]+"]\n");
			sb.append("  new ATR(deg)=["+new_degrees[0]+", "+new_degrees[1]+", "+new_degrees[2]+"]\n");
        	if (apply_imu_orient) {
        		for (int i = 0; i < new_ims_mount_atr.length; i++) {
        			if (Math.abs(new_ims_mount_atr[i]) > quat_max_change) {
        				apply_imu_orient = false;
        				sb.append("*** IMU mount angle ["+i+"]=="+new_ims_mount_atr[i]+
        						" exceeds the specified limit ("+quat_max_change+")\n");
        				sb.append("*** Orientation update is disabled.\n");
        			}
        		}
        	}        		
        	if (apply_imu_orient) { 
        		clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
        		sb.append("*** IMU mount angles updated, need to save the main configuration file for persistent storage ***\n");
        	} else {
        		sb.append("*** IMU mount angles calculated, but not applied ***\n");
        	}
			double [] new_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
			double [] degrees = new double[3];
			for (int i = 0; i < 3; i++) degrees[i]=new_atr[i]*180/Math.PI;
			sb.append(String.format(
					"Using ATR(rad)=[%9f, %9f, %9f]\n", new_atr[0], new_atr[1], new_atr[2]));
			sb.append(String.format(
					"Using ATR(deg)=[%9f, %9f, %9f]\n", degrees[0], degrees[1], degrees[2]));
			
        	double [] omega_corr = null;
        	// Work later on adjust_gyro and adjust_accl;
        	/*
        	if (adjust_gyro) {
        		omega_corr = getOmegaCorrections(
        				clt_parameters,     // CLTParameters clt_parameters,     // CLTParameters clt_parameters,
        				rotated_xyzatr,     // double [][][] rotated_xyzatr,
        				quadCLTs,           // QuadCLT[]     quadCLTs,
        				ref_index,          // int           ref_index,
        				earliest_scene,     // int           earliest_scene,
        				last_index,         // int           last_index,
        				debugLevel);        // int debugLevel
        		if (omega_corr != null) {
    				double [] used_omegas = clt_parameters.imp.get_pimu_offs()[1]; // returns in atr order
    				double [] new_omegas =  new double[3];
    				for (int i = 0; i < 3; i++) {
    					new_omegas[i] = used_omegas[i] - omega_corr[i];        					
    				}
    				sb.append(String.format(
    						"Used omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
    						used_omegas[0],used_omegas[1],used_omegas[2]));
    				sb.append(String.format(
    						"Diff.omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
    						omega_corr[0], omega_corr[1], omega_corr[2]));
    				sb.append(String.format(
    						"New  omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
    						new_omegas[0], new_omegas[1], new_omegas[2]));
        			if (apply_gyro) {
        				clt_parameters.imp.set_pimu_omegas(new_omegas);
        				sb.append(String.format(
        						"Applied new gyro omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
        						new_omegas[0], new_omegas[1], new_omegas[2]));
        				sb.append("*** Need to save the main configuration file ***\n");
        			} else {
        				sb.append(String.format(
        						"New gyro omegas (ATR, rad/s) are not applied = [%9f, %9f, %9f]\n",
        						new_omegas[0], new_omegas[1], new_omegas[2]));
        			}
        		} else {
        			sb.append("*** Adjustment of the gyro omegas failed ***\n");
        		}
        	}
        	double [] acc_corr = null;
        	if (adjust_accl) {
        		acc_corr = getVelocitiesCorrections( // > 1 when IMU is larger than camera
        				clt_parameters,     // CLTParameters clt_parameters,     // CLTParameters clt_parameters,
        				rotated_xyzatr,     // double [][][] rotated_xyzatr,
        				quadCLTs,           // QuadCLT[]     quadCLTs,
        				ref_index,          // int           ref_index,
        				earliest_scene,     // int           earliest_scene,
        				last_index,         // int           last_index,
        				quat_min_lin, // double        min_range,
        				debugLevel);        // int debugLevel
        		
        		if (acc_corr != null) {
    				double [] used_accl_corr = clt_parameters.imp.get_pimu_offs()[0];
    				double [] new_accl_corr =  used_accl_corr.clone();
    				int num_corr=0;
    				for (int i = 0; i < 3; i++) if (!Double.isNaN(acc_corr[i])){
    					new_accl_corr[i] = used_accl_corr[i] * acc_corr[i];
    					num_corr++;
    				}
    				sb.append(String.format(
    						"Used velocities scales = [%9f, %9f, %9f]\n",
    						used_accl_corr[0],used_accl_corr[1],used_accl_corr[2]));
    				sb.append(String.format(
    						"Diff.velocities scales = [%9f, %9f, %9f]\n",
    						acc_corr[0], acc_corr[1], acc_corr[2]));
    				sb.append(String.format(
    						"New  velocities scales = [%9f, %9f, %9f]\n",
    						new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
        			if (apply_accl && (num_corr>0)) {
        				clt_parameters.imp.set_pimu_velocities_scales(new_accl_corr);
        				sb.append(String.format(
        						"Applied new velocities scales = [%9f, %9f, %9f]\n",
        						new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
        				sb.append("*** Need to save the main configuration file ***\n");
        			} else {
        				sb.append(String.format(
        						"New velocities scales are not applied = [%9f, %9f, %9f]\n",
        						new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
        			}
        		} else {
        			sb.append("*** Adjustment of the gyro omegas failed ***\n");
        		}
        		//
        	}
        	*/
        	sb.append("------------------------\n\n");
        	index_scene.appendStringInModelDirectory(sb.toString(),QuadCLTCPU.IMU_CALIB_LOGS_SUFFIX); // String  suffix)
        	if (debugLevel > -3) {
        		System.out.print(sb.toString());
        	}
    	} else {
    		System.out.println ("*** Failed to calculate IMS mount correction! ***");
    	}
    	return quat;
    }

    
    public static double[] adjustImuOrient(  // single-segment
    		CLTParameters clt_parameters,     // CLTParameters clt_parameters,
    		QuadCLT[]     quadCLTs,
    		int           ref_index,
    		boolean       save_details,
    		int           debugLevel) {
//    	@Deprecated
//		boolean orient_by_move =    clt_parameters.imp.orient_by_move; // use translation data to adjust IMU orientation
//    	@Deprecated
//		boolean orient_by_rot =     clt_parameters.imp.orient_by_rot; // use rotation data to adjust IMU orientation
//		if (!orient_by_move && !orient_by_rot) {
//			System.out.println("Neither translation nor rotation data are enabled to adjust IMU orientation relative to the camera");
//			return null;
//		}
//		boolean orient_combo =     clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
		boolean apply_imu_orient =  clt_parameters.imp.apply_imu_orient; // apply  IMU misalignment to the camera if adjusted
		boolean adjust_gyro =       clt_parameters.imp.adjust_gyro; // adjust qyro omegas offsets
		boolean apply_gyro =        clt_parameters.imp.apply_gyro; // apply adjusted qyro omegas offsets
		boolean adjust_accl =       clt_parameters.imp.adjust_accl; // adjust IMU velocities scales
		boolean apply_accl =        clt_parameters.imp.apply_accl; // apply IMU velocities scales
		double  quat_max_change =   clt_parameters.imp.quat_max_change; // do not apply if any component of the result exceeds
		double  quat_min_lin =      clt_parameters.imp.quat_min_lin; // meters, minimal distance per axis to adjust IMS velocity scale
//		boolean use3=               clt_parameters.imp.use3; // false; // false - full4 quaternion (with scale), true - only q1,q2,q3
		double  transl_cost =       clt_parameters.imp.wser_orient_transl; // AGL corrected, 0.5 - equal weights, 0.0 - rotation only, 1.0 - translation-only
		double  compass_cost =       clt_parameters.imp.wser_orient_compass; // weight of IMS compass angle relative to others (1.0 - all equal)
		
		
		// calculate average Z for the sequence (does not need to be very accurate
	
		double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters 
    	double [] rms = new double[7];
    	double [] quat = new double[4];
    	int debug_lev = debugLevel; // 3;
    	if (debugLevel > -3) {
    		System.out.println("adjustImuOrient(): transl_cost="+transl_cost+", compass_cost="+compass_cost);
    	}
		ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();
		double [][][] pimu_xyzatr =  QuadCLT.integratePIMU(
				clt_parameters,          // final CLTParameters clt_parameters,
				quadCLTs,                // final QuadCLT[]     quadCLTs,
				null, // quat_corr,      // double []  quat_corr,
				ref_index,               // final int           ref_index,
				null,                    // double [][][]       dxyzatr,
				0, // earliest_scene,    // final int           early_index,
				quadCLTs.length -1,      //  last_index,     //(quadCLTs.length -1) // int           last_index,
				debugLevel);             // final int           debugLevel
		double [][][] xyzatr =  new double [quadCLTs.length][][];
		for (int nscene = 0; nscene < xyzatr.length; nscene++) {
			String ts = quadCLTs[nscene].getImageName();
			xyzatr[nscene] = ers_ref.getSceneXYZATR(ts);
		}
    	String        suffix = save_details ? QuadCLTCPU.IMU_CALIB_DETAILS_SUFFIX : null;
    	double [] y_scale_p = new double[1];
    	
    	double [][][] rotated_xyzatrs = rotateImsToCameraXYZ(
    			clt_parameters,     // CLTParameters clt_parameters,
    			avg_z,              // double        avg_height,
    			transl_cost,        // translation_weight, // double        translation_weight,
    			compass_cost,       // double        compass_cost, // 0.05;  // weight of IMS compass angle relative to others (1.0 - all equal)
    			quadCLTs,           // QuadCLT[]      quadCLTs,
    			xyzatr,             // double [][][] xyzatr,
    			pimu_xyzatr,        // double [][][] ims_xyzatr,
    			ref_index,          // int           ref_index,
    			rms,                // double []     rms, // null or double[5];
    			quat,               // double []     quaternion, // null or double[4]
    			y_scale_p,          // double [] y_scale_p
    			suffix,             // String        suffix,
    			debug_lev);         // int           debugLevel
    	if (rotated_xyzatrs != null) {
        	Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
        	double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
        	double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(
        			ims_mount_atr, // double [] ims_atr,
        			quat); // double [] corr_q)
        	StringBuffer sb = new StringBuffer();
        	sb.append(new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(Calendar.getInstance().getTime())+"\n");
    		sb.append("Applying correction to the IMS mount orientation:\n");
        	sb.append("avg_z= "+avg_z+" m\n");
        	sb.append("transl_cost=   "+transl_cost+"\n");
        	sb.append("compass_cost=  "+compass_cost+"\n");        	
        	sb.append("y_scale=       "+y_scale_p[0]+"\n");
        	sb.append("RMSe=");
        	for (int i = 0; i < rms.length-1; i++) sb.append(rms[i]+",");
        	sb.append(rms[rms.length-1]+"\n");
 			double []  corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
			double []  corr_degrees = new double[3];
			double quat_scale = Math.sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]);
			for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
			double [] new_degrees = new double[3];
			for (int i = 0; i < 3; i++) new_degrees[i]=new_ims_mount_atr[i]*180/Math.PI;
			sb.append("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
			sb.append("scale="+quat_scale+"\n");
			double angle = Math.sqrt(corr_angles[0]*corr_angles[0] + corr_angles[1]*corr_angles[1] + corr_angles[2]*corr_angles[2]);
			sb.append("delta ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"] ("+ angle+")\n");
			sb.append("delta ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"] ("+ angle*180/Math.PI+")\n");
			sb.append("  new ATR(rad)=["+new_ims_mount_atr[0]+", "+new_ims_mount_atr[1]+", "+new_ims_mount_atr[2]+"]\n");
			sb.append("  new ATR(deg)=["+new_degrees[0]+", "+new_degrees[1]+", "+new_degrees[2]+"]\n");
        	if (apply_imu_orient) {
        		for (int i = 0; i < new_ims_mount_atr.length; i++) {
        			if (Math.abs(new_ims_mount_atr[i]) > quat_max_change) {
        				apply_imu_orient = false;
        				sb.append("*** IMU mount angle ["+i+"]=="+new_ims_mount_atr[i]+
        						" exceeds the specified limit ("+quat_max_change+")\n");
        				sb.append("*** Orientation update is disabled.\n");
        			}
        		}
        	}        		
        	if (apply_imu_orient) { 
        		clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
        		sb.append("*** IMU mount angles updated, need to save the main configuration file for persistent storage ***\n");
        	} else {
        		sb.append("*** IMU mount angles calculated, but not applied ***\n");
        	}
			double [] new_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
			double [] degrees = new double[3];
			for (int i = 0; i < 3; i++) degrees[i]=new_atr[i]*180/Math.PI;
			sb.append(String.format(
					"Using ATR(rad)=[%9f, %9f, %9f]\n", new_atr[0], new_atr[1], new_atr[2]));
			sb.append(String.format(
					"Using ATR(deg)=[%9f, %9f, %9f]\n", degrees[0], degrees[1], degrees[2]));
			
        	double [] omega_corr = null;
        	// Work later on adjust_gyro and adjust_accl;
        	/*
        	if (adjust_gyro) {
        		omega_corr = getOmegaCorrections(
        				clt_parameters,     // CLTParameters clt_parameters,     // CLTParameters clt_parameters,
        				rotated_xyzatr,     // double [][][] rotated_xyzatr,
        				quadCLTs,           // QuadCLT[]     quadCLTs,
        				ref_index,          // int           ref_index,
        				earliest_scene,     // int           earliest_scene,
        				last_index,         // int           last_index,
        				debugLevel);        // int debugLevel
        		if (omega_corr != null) {
    				double [] used_omegas = clt_parameters.imp.get_pimu_offs()[1]; // returns in atr order
    				double [] new_omegas =  new double[3];
    				for (int i = 0; i < 3; i++) {
    					new_omegas[i] = used_omegas[i] - omega_corr[i];        					
    				}
    				sb.append(String.format(
    						"Used omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
    						used_omegas[0],used_omegas[1],used_omegas[2]));
    				sb.append(String.format(
    						"Diff.omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
    						omega_corr[0], omega_corr[1], omega_corr[2]));
    				sb.append(String.format(
    						"New  omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
    						new_omegas[0], new_omegas[1], new_omegas[2]));
        			if (apply_gyro) {
        				clt_parameters.imp.set_pimu_omegas(new_omegas);
        				sb.append(String.format(
        						"Applied new gyro omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
        						new_omegas[0], new_omegas[1], new_omegas[2]));
        				sb.append("*** Need to save the main configuration file ***\n");
        			} else {
        				sb.append(String.format(
        						"New gyro omegas (ATR, rad/s) are not applied = [%9f, %9f, %9f]\n",
        						new_omegas[0], new_omegas[1], new_omegas[2]));
        			}
        		} else {
        			sb.append("*** Adjustment of the gyro omegas failed ***\n");
        		}
        	}
        	double [] acc_corr = null;
        	if (adjust_accl) {
        		acc_corr = getVelocitiesCorrections( // > 1 when IMU is larger than camera
        				clt_parameters,     // CLTParameters clt_parameters,     // CLTParameters clt_parameters,
        				rotated_xyzatr,     // double [][][] rotated_xyzatr,
        				quadCLTs,           // QuadCLT[]     quadCLTs,
        				ref_index,          // int           ref_index,
        				earliest_scene,     // int           earliest_scene,
        				last_index,         // int           last_index,
        				quat_min_lin, // double        min_range,
        				debugLevel);        // int debugLevel
        		
        		if (acc_corr != null) {
    				double [] used_accl_corr = clt_parameters.imp.get_pimu_offs()[0];
    				double [] new_accl_corr =  used_accl_corr.clone();
    				int num_corr=0;
    				for (int i = 0; i < 3; i++) if (!Double.isNaN(acc_corr[i])){
    					new_accl_corr[i] = used_accl_corr[i] * acc_corr[i];
    					num_corr++;
    				}
    				sb.append(String.format(
    						"Used velocities scales = [%9f, %9f, %9f]\n",
    						used_accl_corr[0],used_accl_corr[1],used_accl_corr[2]));
    				sb.append(String.format(
    						"Diff.velocities scales = [%9f, %9f, %9f]\n",
    						acc_corr[0], acc_corr[1], acc_corr[2]));
    				sb.append(String.format(
    						"New  velocities scales = [%9f, %9f, %9f]\n",
    						new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
        			if (apply_accl && (num_corr>0)) {
        				clt_parameters.imp.set_pimu_velocities_scales(new_accl_corr);
        				sb.append(String.format(
        						"Applied new velocities scales = [%9f, %9f, %9f]\n",
        						new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
        				sb.append("*** Need to save the main configuration file ***\n");
        			} else {
        				sb.append(String.format(
        						"New velocities scales are not applied = [%9f, %9f, %9f]\n",
        						new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
        			}
        		} else {
        			sb.append("*** Adjustment of the gyro omegas failed ***\n");
        		}
        		//
        	}
        	*/
        	sb.append("------------------------\n\n");
        	quadCLTs[ref_index].appendStringInModelDirectory(sb.toString(),QuadCLTCPU.IMU_CALIB_LOGS_SUFFIX); // String  suffix)
        	if (debugLevel > -3) {
        		System.out.print(sb.toString());
        	}
    	} else {
    		System.out.println ("*** Failed to calculate IMS mount correction! ***");
    	}
    	return quat;
    }

        
    
    
    
    public static double [][][][] rotateImsToCameraXYZ( // multi-segment
    		CLTParameters clt_parameters,
        	double        avg_height,
        	double        transl_cost,  // 1.0 - pure translation, 0.0 - pure rotation
			double        compass_cost, // 0.05;  // weight of IMS compass angle relative to others (1.0 - all equal)
    		QuadCLT[][]     quadCLTss,
    		double [][][][] xyzatrs,
    		double [][][][] ims_xyzatrs,
    		int []          ref_indices,
			double []     rms, // null or double[5];
			double []     quaternion, // null or double[4]
			double []     y_scale_p,
			QuadCLT       index_scene, // where to put result
			String        suffix,
			int           debugLevel) {
    	boolean use_inv = true; // false; //
    	double [][][] xyzatr =     flattenPoses(xyzatrs);
    	double [][][] ims_xyzatr = flattenPoses(ims_xyzatrs);
    	double [] quat = getRotationFromXYZATRCameraIms(
    			clt_parameters,     // CLTParameters   clt_parameters,
    			avg_height,         // double          avg_height,  // for scaling translational components  			
    			transl_cost,        // double          transl_cost,
    			compass_cost,       // double        compass_cost, // 0.05;  // weight of IMS compass angle relative to others (1.0 - all equal)
    			xyzatr,             // double [][][]   xyzatr,
    			ims_xyzatr,         // double [][][]   ims_xyzatr,
    			rms,                // double []       rms, // null or double[5];
    			y_scale_p,          // double []       y_scale_p, // if not null will return y_scale
    			debugLevel);        // int           debugLevel

    	if (quat == null) {
    		return null;
    	}
    	if (quaternion != null) {
    		System.arraycopy(quat, 0, quaternion, 0, 4);
    	}
    	Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
		Rotation rot_invert = rot.revert();
    	int num_refs = ims_xyzatrs.length;
    	double [][][][] rot_ims_xyzatrs = new double [num_refs][][][]; // ims poses rotated by the new correction to match camera ones
    	for (int nref = 0; nref < num_refs; nref++) {
    		int num_scenes = ims_xyzatrs[nref].length;
    		rot_ims_xyzatrs[nref] = new double [num_scenes][][]; // orientation from camera, xyz from rotated IMS
    		double [] rot_ims_xyz = new double[3];
    		for (int nscene = 0; nscene < num_scenes; nscene++) {
    			rot.applyTo(ims_xyzatrs[nref][nscene][0], rot_ims_xyz); // xyz from ims rotated to match the camera
    			// not used
    			Rotation scene_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
    					xyzatrs[nref][nscene][1][0],    xyzatrs[nref][nscene][1][1],    xyzatrs[nref][nscene][1][2]);
    			// convert ims angles to quaternion:
    			Rotation rot_ims = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
    					ims_xyzatrs[nref][nscene][1][0],ims_xyzatrs[nref][nscene][1][1],ims_xyzatrs[nref][nscene][1][2]);
    			Rotation rot_corr_ims,rotation_atr2;
    			if (use_inv) { // should not be used
    				rot_corr_ims = rot_invert.applyTo(rot_ims);
    				rotation_atr2 = rot_corr_ims.applyTo(rot); // applyInverseTo?
    			} else {
    				rot_corr_ims = rot.applyTo(rot_ims); // applying correction to the IMS orientation
    				rotation_atr2 = rot_corr_ims.applyTo(rot_invert); // applyInverseTo?
    			}
    			rot_ims_xyzatrs[nref][nscene] = new double [][] {rot_ims_xyz.clone(),
    				rotation_atr2.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
    		}
    	}
        if (suffix != null) {
    	    Path orient_path = Paths.get(index_scene.getX3dDirectory(true)).
    	    		resolve(index_scene.getImageName()+suffix); // IMU_CALIB_DETAILS_SUFFIX);
        	saveRotateImsMultiDetails(
        			rot,                     // Rotation      rot,
        			xyzatrs,                  // double [][][] xyzatr,
        			ims_xyzatrs,              // double [][][] ims_xyzatr,
        			rot_ims_xyzatrs,          // double [][][] rotated_xyzatr,
        			y_scale_p[0],             // double          y_scale,
        			orient_path.toString()); // String        path) {
    		if (debugLevel > -3) {
    			System.out.println("Orientation details saved to "+orient_path.toString());
    		}
        }
    	return rot_ims_xyzatrs;
    }
    
    public static double [][][] rotateImsToCameraXYZ( // single-segment
    		CLTParameters clt_parameters,
        	double        avg_height,
        	double        transl_cost,  // 1.0 - pure translation, 0.0 - pure rotation
        	double        compass_cost, // 0.05;  // weight of IMS compass angle relative to others (1.0 - all equal)
    		QuadCLT[]     quadCLTs,
    		double [][][] xyzatr,
    		double [][][] ims_xyzatr,
    		int           ref_index,
			double []     rms, // null or double[5];
			double []     quaternion, // null or double[4]
			double []     y_scale_p,
			String        suffix,
			int           debugLevel) {
    	boolean use_inv = true; // false; //
    	double [] quat = getRotationFromXYZATRCameraIms(
    			clt_parameters,     // CLTParameters   clt_parameters,
    			avg_height,         // double          avg_height,  // for scaling translational components  			
    			transl_cost,        // double          transl_cost,
    			compass_cost,       // double        compass_cost, // 0.05;  // weight of IMS compass angle relative to others (1.0 - all equal)
    			xyzatr,             // double [][][]   xyzatr,
    			ims_xyzatr,         // double [][][]   ims_xyzatr,
    			rms,                // double []       rms, // null or double[5];
    			y_scale_p,          // double []       y_scale_p, // if not null will return y_scale
    			debugLevel);        // int           debugLevel

    	if (quat == null) {
    		return null;
    	}
    	if (quaternion != null) {
    		System.arraycopy(quat, 0, quaternion, 0, 4);
    	}
    	Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
    	Rotation rot_invert = rot.revert();
    	int num_scenes = ims_xyzatr.length;
    	double [][][] rot_ims_xyzatr = new double [num_scenes][][]; // orientation from camera, xyz from rotated IMS
    	double [] rot_ims_xyz = new double[3];
    	for (int nscene = 0; nscene < num_scenes; nscene++) {
    		rot.applyTo(ims_xyzatr[nscene][0], rot_ims_xyz); // xyz from ims rotated to match the camera
    		// not used
    		//    			Rotation scene_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
    		//    					xyzatrs[nref][nscene][1][0],    xyzatrs[nref][nscene][1][1],    xyzatrs[nref][nscene][1][2]);
    		// convert ims angles to quaternion:
    		Rotation rot_ims = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
    				ims_xyzatr[nscene][1][0], ims_xyzatr[nscene][1][1],ims_xyzatr[nscene][1][2]);
    		Rotation rot_corr_ims,rotation_atr2;
    		if (use_inv) { // should not be used
    			rot_corr_ims = rot_invert.applyTo(rot_ims);
    			rotation_atr2 = rot_corr_ims.applyTo(rot); // applyInverseTo?
    		} else {
    			rot_corr_ims = rot.applyTo(rot_ims); // applying correction to the IMS orientation
    			rotation_atr2 = rot_corr_ims.applyTo(rot_invert); // applyInverseTo?
    		}
    		rot_ims_xyzatr[nscene] = new double [][] {rot_ims_xyz.clone(),
    			rotation_atr2.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
    	}
        if (suffix != null) {
    	    Path orient_path = Paths.get(quadCLTs[ref_index].getX3dDirectory(true)).
    	    		resolve(quadCLTs[ref_index].getImageName()+suffix); // IMU_CALIB_DETAILS_SUFFIX);
    	    saveRotateImsDetails(
        			rot,            // Rotation      rot,
        			xyzatr,         // double [][][] xyzatr,
        			ims_xyzatr,     // double [][][] ims_xyzatr,
        			rot_ims_xyzatr, // double [][][] rotated_xyzatr,
        			y_scale_p[0],            // double        y_scale,
        			orient_path.toString()); // String        path) {
    		if (debugLevel > -3) {
    			System.out.println("Orientation details saved to "+orient_path.toString());
    		}
        }
    	return rot_ims_xyzatr;
    }
    
    
    /**
     * Flatten multi-segment sequences to a single segment ones. Need to apply to double [][][][] xyzatrs and
     * double [][][][] ims_xyzatrs separately
     * @param pose_segments array of poses to be flattened [segment_index][scene_index]{{x,y,z},{a,t,r}}
     * @return flattened poses [scene_index]{{x,y,z},{a,t,r}}
     */
    public static double [][][] flattenPoses(
    	double [][][][] pose_segments) {
		int num_samples = 0;
		for (int nref = 0; nref < pose_segments.length;nref++) {
			num_samples += pose_segments[nref].length;
		}
		double [][][] poses = new double [num_samples][][];
		int vindx = 0;
		for (int nref = 0; nref < pose_segments.length;nref++) {
			for (int nscene = 0; nscene <  pose_segments[nref].length; nscene++) {
				poses[vindx] =    pose_segments[nref][nscene];
				vindx++;
			}
		}
		return poses;
    }

    /**
     * @param clt_parameters configuration parameters
     * @param quadCLTs       scenes sequence (for timestamps)
     * @param avr_height     for scaling translational components
     * @param transl_cost    relative weight of translations (0..1), rotation weighs: 1-translation_weight
     * @param compass_cost   reduce weight of the IMS compass angle compare to the two others, as IMS compass
     *                       is less accurate. Use translation instead. compass_cost=1.0 - same weight as other components.
     *                       Currently this applies just to quaternion [3], it approximately matches downward camera roll.
     *                       May be improved to apply to the actual IMS up/down axis.
     * @param vect_y         scenes relative (to their references) poses {{x,y,z},{a,t,r]}}
     * @param vect_x         IMU-derived (integrated) scene position and orientation relative to its reference
     * @param rms            null or a double[7] to receive rms values 
     * @param y_scale_p      null or double[1] - will return y_scale - camera translations relative to the IMS
     * @param debugLevel     debug level
     * @return quaternion    components 
     */
    public static double [] getRotationFromXYZATRCameraIms(
    		CLTParameters   clt_parameters,
    		double          avg_height,  // for scaling translational components 
    		double          transl_cost, // will be relative weight of translations (0..1), rotations - 1-translation_weight
			double          compass_cost, // 0.05;  // weight of IMS compass angle relative to others (1.0 - all equal)
    		double [][][]   vect_y,
    		double [][][]   vect_x,
			double []       rms, // null or double[5];
			double []       y_scale_p, // if not null will return y_scale
			int             debugLevel) {
    	boolean print_lma_table = false;
    	boolean print_lma_table_atr = true; // add ATR
		double lambda =            clt_parameters.imp.quat_lambda; // 0.1;
		double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5;
		double lambda_scale_bad =  clt_parameters.imp.quat_lambda_scale_bad; // 8.0;
		double lambda_max =        clt_parameters.imp.quat_lambda_max; // 100;
		double rms_diff =          clt_parameters.imp.quat_rms_diff; // 0.001;
		int    num_iter =          clt_parameters.imp.quat_num_iter; // 20;
		boolean last_run =         false;
		double  reg_w =            clt_parameters.imp.quat_reg_w; // 0.25;
		double  quat_max_ratio =   clt_parameters.imp.quat_max_ratio; // 0.25;
		double  quat_max_damping = clt_parameters.imp.quat_max_damping;
		boolean use_scale_y =      clt_parameters.imp.quat_scale_y;
		double xyz_scale =         1.0 / (avg_height + 1.0);
		// scale translational components (x,y,z) to match rotational one by the "order of magnitude"
		// use clone() to keep the source data intact.
		for (int nscene = 0; nscene <  vect_x.length; nscene++) {
			vect_x[nscene] = vect_x[nscene].clone();
			vect_y[nscene] = vect_y[nscene].clone();
			vect_x[nscene][0] = vect_x[nscene][0].clone();
			vect_y[nscene][0] = vect_y[nscene][0].clone();
			for (int i = 0; i < 3; i++) {
				vect_x[nscene][0][i] *= xyz_scale;
				vect_y[nscene][0][i] *= xyz_scale;
			}
		}		
		
		double [][][] vect_y_scaled;
		double y_scale = 1.0;
		if (use_scale_y) {
			y_scale = XyzQLma.getLengthRatio(
					vect_x,      // double [][][] vect_x, // []{{x,y,z},{a,t,r}}
					vect_y,      // double [][][] vect_y, //  []{{x,y,z},{a,t,r}}
					null);       // new int [] {early_index, last_index}); // int []    first_last){ // null - all
			
			vect_y_scaled= XyzQLma.scaleXYZ(
					1.0/y_scale, // double        s,
					vect_y,      // double [][][] vect_y, //  []{{x,y,z},{a,t,r}}
					null);       // new int [] {early_index, last_index}); // int []    first_last){ // null - all
		} else {
			vect_y_scaled = vect_y;
		}
		if (y_scale_p != null) {
			y_scale_p[0] = y_scale;
		}
		
		
		XyzQLma xyzQLma = new XyzQLma();
		xyzQLma.prepareLMA(
				vect_x,             // double [][][] vect_x,
				vect_y_scaled,      // vect_y,       // double [][][] vect_y,
				null,               // double []     vect_w, all same weight
				transl_cost,        // double        translation_weight, // 0.0 ... 1.0;
				compass_cost,       // double        compass_cost, // 0.0 ... 1.0;
				0.0,                // reg_w,        // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  
				debugLevel);        // int           debug_level)
		double [] mn_mx_diag = xyzQLma.getMinMaxDiag(debugLevel);
		double max_to_min = Math.sqrt(mn_mx_diag[1]/mn_mx_diag[0]);
		if (debugLevel > -3) {
			System.out.println("max_to_min = "+max_to_min+", quat_max_ratio="+quat_max_ratio);
		}
		//			if (mn_mx_diag[0]*quat_max_ratio*quat_max_ratio < mn_mx_diag[1]) {
		if (max_to_min > quat_max_ratio) {
			reg_w = quat_max_damping * (max_to_min - quat_max_ratio) / max_to_min;
			double a = mn_mx_diag[1]; // Math.sqrt(mn_mx_diag[1]);
			double reg_w_old = a/(a + quat_max_ratio*quat_max_ratio/4); // quat0.length);
			if (debugLevel > -3) {
				System.out.println("====2 Calculated reg_w = "+reg_w+", reg_w_old="+reg_w_old+", a="+a);
			}
			xyzQLma.prepareLMA(
					vect_x,             // double [][][] vect_x,
					vect_y_scaled,      // vect_y,             // double [][][] vect_y,
					null,               // double []     vect_w, all same weight
					transl_cost,        // double        translation_weight, // 0.0 ... 1.0;
					compass_cost,       // double        compass_cost, // 0.0 ... 1.0;
					reg_w,              // double        reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  
					debugLevel);        // int           debug_level)
		}
		
		if (print_lma_table) {
			xyzQLma.saveFx(debugLevel);	
		}
		
		int lma_result = xyzQLma.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,
				debugLevel);     // int    debug_level)
		if (lma_result < 0) {
			return null;
		} else {
			if (print_lma_table) {
				String lma_table= xyzQLma.getLmaTable(
						print_lma_table_atr,
						"\t",
						debugLevel);
				System.out.println(lma_table);
			}

			if (rms != null) { // null or double[2];
				double [] last_rms = xyzQLma.getLastRms();
				rms[0] = last_rms[0];
				rms[1] = last_rms[1];
				if (rms.length >= 4) {
					double [] initial_rms = xyzQLma.getInitialRms();
					rms[2] = initial_rms[0];
					rms[3] = initial_rms[1];
					if (rms.length >= 5) {
						rms[4] = lma_result;
						if ((rms.length >= 6) && (last_rms.length > 2)){
							rms[5] = last_rms[2];
							if ((rms.length >= 7) && (last_rms.length > 3)){
								rms[6] = last_rms[3];
							}
						}
					}					
				}
			}
			return xyzQLma.getQuaternion();
		}
    }
    
    public static void saveRotateImsMultiDetails(
    		Rotation        rot,
    		double [][][][] xyzatrs,
    		double [][][][] ims_xyzatrs,
    		double [][][][] rotated_xyzatrs,
    		double          y_scale,
    		String          path) {
		double []  angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
		double []  degrees = new double[3];
		for (int i = 0; i < 3; i++) degrees[i]=angles[i]*180/Math.PI;
		StringBuffer sb = new StringBuffer();
		sb.append("quat\t"+rot.getQ0()+"\t"+rot.getQ1()+"\t"+rot.getQ2()+"\t"+rot.getQ3()+"\n");
		sb.append("ATR(rad)\t"+angles[0]+"\t"+angles[1]+"\t"+angles[2]+"\n");
		sb.append("ATR(deg)\t"+degrees[0]+"\t"+degrees[1]+"\t"+degrees[2]+"\n");
		sb.append("y_scale=\t"+y_scale+"\n");
		sb.append(
				"N\tCAM-X\tCAM-Y\tCAM-Z\tCAM-A\tCAM-T\tCAM-R\t"+
				"IMS-X\tIMS-Y\tIMS-Z\tIMS-A\tIMS-T\tIMS-R\t"+
				"RIMS-X\tRIMS-Y\tRIMS-Z\tRIMS-A\tRIMS-T\tRIMS-R\n");
		int num_refs = xyzatrs.length;
		for (int nref = 0; nref < num_refs; nref++) {
			int num_scenes = xyzatrs[nref].length;
			for (int nscene = 0; nscene < num_scenes; nscene++) {
				sb.append(String.format("%3d"+
						"\t%f\t%f\t%f\t%f\t%f\t%f"+
						"\t%f\t%f\t%f\t%f\t%f\t%f"+
						"\t%f\t%f\t%f\t%f\t%f\t%f\n",
						nscene,
						xyzatrs[nref][nscene][0][0],xyzatrs[nref][nscene][0][1],xyzatrs[nref][nscene][0][2],
						xyzatrs[nref][nscene][1][0],xyzatrs[nref][nscene][1][1],xyzatrs[nref][nscene][1][2],
						ims_xyzatrs[nref][nscene][0][0],ims_xyzatrs[nref][nscene][0][1],ims_xyzatrs[nref][nscene][0][2],
						ims_xyzatrs[nref][nscene][1][0],ims_xyzatrs[nref][nscene][1][1],ims_xyzatrs[nref][nscene][1][2],
						rotated_xyzatrs[nref][nscene][0][0],rotated_xyzatrs[nref][nscene][0][1],rotated_xyzatrs[nref][nscene][0][2],
						rotated_xyzatrs[nref][nscene][1][0],rotated_xyzatrs[nref][nscene][1][1],rotated_xyzatrs[nref][nscene][1][2]));
			}
		}
		CalibrationFileManagement.saveStringToFile (
				path,
				sb.toString());
    }
 
    public static void saveRotateImsDetails(
    		Rotation      rot,
    		double [][][] xyzatr,
    		double [][][] ims_xyzatr,
    		double [][][] rotated_xyzatr,
    		double        y_scale,
    		String        path) {
		double []  angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
		double []  degrees = new double[3];
		for (int i = 0; i < 3; i++) degrees[i]=angles[i]*180/Math.PI;
		StringBuffer sb = new StringBuffer();
		sb.append("quat\t"+rot.getQ0()+"\t"+rot.getQ1()+"\t"+rot.getQ2()+"\t"+rot.getQ3()+"\n");
		sb.append("ATR(rad)\t"+angles[0]+"\t"+angles[1]+"\t"+angles[2]+"\n");
		sb.append("ATR(deg)\t"+degrees[0]+"\t"+degrees[1]+"\t"+degrees[2]+"\n");
		sb.append("y_scale=\t"+y_scale+"\n");
		sb.append(
				"N\tCAM-X\tCAM-Y\tCAM-Z\tCAM-A\tCAM-T\tCAM-R\t"+
				"IMS-X\tIMS-Y\tIMS-Z\tIMS-A\tIMS-T\tIMS-R\t"+
				"RIMS-X\tRIMS-Y\tRIMS-Z\tRIMS-A\tRIMS-T\tRIMS-R\n");
		int num_scenes = xyzatr.length;
		for (int nscene = 0; nscene < num_scenes; nscene++) {
			sb.append(String.format("%3d"+
					"\t%f\t%f\t%f\t%f\t%f\t%f"+
					"\t%f\t%f\t%f\t%f\t%f\t%f"+
					"\t%f\t%f\t%f\t%f\t%f\t%f\n",
					nscene,
					xyzatr[nscene][0][0],xyzatr[nscene][0][1],xyzatr[nscene][0][2],
					xyzatr[nscene][1][0],xyzatr[nscene][1][1],xyzatr[nscene][1][2],
					ims_xyzatr[nscene][0][0],ims_xyzatr[nscene][0][1],ims_xyzatr[nscene][0][2],
					ims_xyzatr[nscene][1][0],ims_xyzatr[nscene][1][1],ims_xyzatr[nscene][1][2],
					rotated_xyzatr[nscene][0][0],rotated_xyzatr[nscene][0][1],rotated_xyzatr[nscene][0][2],
					rotated_xyzatr[nscene][1][0],rotated_xyzatr[nscene][1][1],rotated_xyzatr[nscene][1][2]));
		}
		CalibrationFileManagement.saveStringToFile (
				path,
				sb.toString());
    }
     
	public static double [] adjustImuOrientMulti(
			CLTParameters    clt_parameters,
			QuadCLT []       refCLTs,
			QuadCLT          index_CLT, // normally - just the last in quadCLTs
			boolean          save_sequence_orient,
			boolean          process_segments,
			double [][]      orient_rslt,
			final int        debugLevel) {
//		boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
//		boolean orient_combo =     clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
		
		for (QuadCLT ref_scene:refCLTs) {
			ref_scene.restoreAnyDSI(debugLevel);
		}
		// get all scenes used in at least one segment.
		HashSet<String> scene_set = new HashSet<String>();
		HashMap<String,QuadCLT> ref_map = new HashMap<String,QuadCLT>(); 
		for (QuadCLT ref_scene:refCLTs) {
			ref_map.put(ref_scene.getImageName(), ref_scene);
			ErsCorrection ers_reference = ref_scene.getErsCorrection();
			String [] fl_ts = ref_scene.getFirstLastTimestamps();
			boolean good_fl = (fl_ts != null) && (fl_ts[0] != null) && (fl_ts[1] !=null);
			String [] names =good_fl ? ers_reference.getScenes(fl_ts[0],fl_ts[1]) : ers_reference.getScenes(); // others, referenced by reference
			for (String s:names)scene_set.add(s);
		}
		String [] all_scene_names = scene_set.toArray(new String[0]);
		Arrays.sort(all_scene_names);
		HashMap<String,QuadCLT> scene_map = new HashMap<String,QuadCLT>(); 
		String models_dir = index_CLT.correctionsParameters.x3dDirectory;
		final String suffix = index_CLT.correctionsParameters.imsSuffix; // assuming common for all
		final QuadCLT[] scenes = new QuadCLT[all_scene_names.length];
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int iScene = ai.getAndIncrement(); iScene < scenes.length; iScene = ai.getAndIncrement()) {
						String scene_name = all_scene_names[iScene];
						QuadCLT rscene = ref_map.get(scene_name);
						// create new if did not exist,
						if (rscene == null) { 
							scenes[iScene] = new QuadCLT(index_CLT,scene_name);
						} else {
							scenes[iScene] = rscene;
						}
						Path ims_path = Paths.get(models_dir).resolve(scene_name).resolve(scene_name+suffix);
						scenes[iScene].restoreIms(
								clt_parameters, // // CLTParameters clt_parameters,
								ims_path.toString(), // String ims_path,
								true, // boolean create,
								debugLevel-1); // debugLevelInner); // int debugLevel);
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		for (int nscene = 0; nscene < all_scene_names.length; nscene++) { // scene_name:all_scene_names) {
			scene_map.put(all_scene_names[nscene], scenes[nscene]);
		}
		// Now iterate through the reference scenes, using IMS data from the scene_map scenes.
		// initially will use earliest and latest scenes only. Improve for the future non-linear flying multicopters
		// and account for sync loss (far samples with random coordinates)
		double [] img_scales =  new double[refCLTs.length];
		double [] dist_visual = new double[refCLTs.length];
		double [] dist_ims2 =   new double[refCLTs.length];
		double [] disp_corrs=   new double[refCLTs.length];
		int []    ref_indices = new int [refCLTs.length];
		double [][] quats =     process_segments? (new double [refCLTs.length][]) : null;
		QuadCLT[][] quadCLTss = new QuadCLT[refCLTs.length][];
		for (int nref = 0; nref < refCLTs.length; nref++) {
			QuadCLT ref_scene = refCLTs[nref];
			ErsCorrection ers_reference = ref_scene.getErsCorrection();
			String [] fl_ts = ref_scene.getFirstLastTimestamps();
			boolean good_fl = (fl_ts != null) && (fl_ts[0] != null) && (fl_ts[1] !=null);
			String [] names =good_fl ? ers_reference.getScenes(fl_ts[0],fl_ts[1]) : ers_reference.getScenes(); // others, referenced by reference
			String name_ref = ref_scene.getImageName();
			QuadCLT[] quadCLTs = new QuadCLT[names.length];
			int ref_index = -1;
			for (int nscene = 0; nscene < quadCLTs.length; nscene++) {
				if (names[nscene].equals(name_ref)) {
					ref_index = nscene;
					quadCLTs[nscene] = ref_scene;
				} else {
					quadCLTs[nscene] = scene_map.get(names[nscene]); 
				}
			}
			if (ref_index < 0) {
				System.out.println("adjustImuOrientMulti(): ref_index <0 for ref_scene "+name_ref);
				continue;
			}
			ref_indices[nref] = ref_index;
			quadCLTss[nref] =   quadCLTs;
			if (ref_scene.getImageName().equals("1763233239_531146")) {
				System.out.println("ref_scene.getImageName()=="+ref_scene.getImageName());
				System.out.println("ref_scene.getImageName()=="+ref_scene.getImageName());
			}
			// just for testing - adjusting per segment
			if (quats != null) {
				/*
					quats [nref] = QuadCLTCPU.adjustImuOrient(
							clt_parameters,      // CLTParameters clt_parameters,     // CLTParameters clt_parameters,
							orient_combo,        // boolean       orient_combo,
							quadCLTs,            // QuadCLT[]     quadCLTs,
							ref_index,           // int           ref_index,
							0,                   // int           earliest_scene,
							quadCLTs.length - 1, // int           last_index,
							save_sequence_orient,// boolean       save_details,
							debugLevel);         // int           debugLevel);
				 */
				quats [nref] = adjustImuOrient( // single-segment
						clt_parameters,         // CLTParameters clt_parameters,
						quadCLTs,               // QuadCLT[]     quadCLTs,
						ref_index,              // int           ref_index,
						save_sequence_orient,   // boolean       save_details,
						debugLevel);            // int           debugLevel);
			}
			continue; // just to put a break point

		}
		// adjusting for all series
		double [] quat = adjustImuOrient( // multi-segment
				clt_parameters,      // CLTParameters clt_parameters,     // CLTParameters clt_parameters,
				quadCLTss,           // QuadCLT[][]   quadCLTss,
				ref_indices,         // int []        ref_indices,
				index_CLT,           // QuadCLT       index_scene, // where to put result
				save_sequence_orient,  // boolean       save_details,
				debugLevel+1);         // int           debugLevel)
		
		// save per-segment quaternion and angles, and the result one
		if (save_sequence_orient && (quats != null)) {
			saveQuaternionCorrection(
					quats,            // double [][]      quats,
					quat,             // double []        quat,
					refCLTs,          // QuadCLT []       refCLTs,
					index_CLT,        // QuadCLT          index_CLT,
					(debugLevel>-3)); // boolean          debug)
		}
		return quat;
	}
	
	public static void saveQuaternionCorrection(
			double [][]      quats,
			double []        quat,
			QuadCLT []       refCLTs,
			QuadCLT          index_CLT,
			boolean          debug) {
		double [][] quats_all = new double[quats.length+1][];
		QuadCLT []  allCLTs = new QuadCLT [quats_all.length]; 
		allCLTs[0] = index_CLT;
		quats_all[0] = quat;
		for (int i = 0; i < quats.length; i++) {
			allCLTs[i+1] = refCLTs[i];
			quats_all[i+1] = quats[i];
		}
		StringBuffer sb = new StringBuffer();
		sb.append("timestamp\tQ0\tQ1\tQ2\tQ3\tscale\tA\tT\tR\tA(deg)\tT(deg)\tR(deg)\n");
		for (int nscene = 0; nscene < quats_all.length; nscene++) {
			double [] q = quats_all[nscene];
        	Rotation rot = new Rotation(q[0],q[1],q[2],q[3], false); // no normalization - see if can be scaled
			double []  angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
			double []  degrees = new double[3];
			double quat_scale = Math.sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
			for (int i = 0; i < 3; i++) degrees[i] = angles[i]*180/Math.PI;
			sb.append(allCLTs[nscene].getImageName()+"\t");
			sb.append(rot.getQ0()+"\t"+rot.getQ1()+"\t"+rot.getQ2()+"\t"+rot.getQ3()+"\t");
			sb.append(quat_scale+"\t");
			sb.append(angles[0]+"\t"+angles[1]+"\t"+angles[2]+"\t");
			sb.append(degrees[0]+"\t"+degrees[1]+"\t"+degrees[2]+"\n");
		}
	   	String        suffix = QuadCLTCPU.IMU_CALIB_SUMMARY_SUFFIX;
	    Path path = Paths.get(index_CLT.getX3dDirectory(true)).resolve(index_CLT.getImageName()+suffix);
		CalibrationFileManagement.saveStringToFile (
				path.toString(),
				sb.toString());
		if (debug) {
			System.out.println("Summary orientation correction data saved to "+path.toString());
		}
	}
	
		
}
