package com.elphel.imagej.tileprocessor;

import java.io.File;
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.common.ShowDoubleFloatArrays;
import com.elphel.imagej.ims.Did_ins_2;
import com.elphel.imagej.ims.Imx5;

public class SeriesInfinityCorrection {
	public static final String SUFFIX_SERIES_INFINITY = "-series_infiniti.csv"; 
	
	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);
				}
				continue; // just to put a break point
				
			}
		// adjusting for all series
		double [] quat = adjustImuOrient(
				clt_parameters,      // CLTParameters clt_parameters,     // CLTParameters clt_parameters,
				orient_combo,        // boolean       orient_combo,
				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)) {
			saveQuternionCorrection(
					quats,            // double [][]      quats,
					quat,             // double []        quat,
					refCLTs,          // QuadCLT []       refCLTs,
					index_CLT,        // QuadCLT          index_CLT,
					orient_combo,     // boolean          orient_combo,
					(debugLevel>-3)); // boolean          debug)
		}
		return quat;
	}
	
	public static void saveQuternionCorrection(
			double [][]      quats,
			double []        quat,
			QuadCLT []       refCLTs,
			QuadCLT          index_CLT,
			boolean          orient_combo,
			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 = orient_combo ? QuadCLTCPU.IMU_CALIB_SUM_COMBO_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());
		}
	}
	
	
	
	
	public static double getInfinityMuliSeries(
    		CLTParameters    clt_parameters,
    		QuadCLT []       refCLTs,
    		QuadCLT          index_CLT, // normally - just the last in quadCLTs
    		double [][]      infinity_rslt,
    		final int        debugLevel) {
		boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
		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];
		for (int nref = 0; nref < refCLTs.length; nref++) {
			QuadCLT ref_scene = refCLTs[nref];
			ErsCorrection ers_reference = ref_scene.getErsCorrection();
			// This does not work, as there can be stale in ers_reference.getScenes();
//			String [] sts = ers_reference.getScenes();
//			String [] first_last_ts = {sts[0],sts[sts.length - 1]};
			String [] first_last_ts = ref_scene.getFirstLastTimestamps();
			
			double[][] xyz_fl = new double [first_last_ts.length][];
			Did_ins_2 [] did_2_fl= new Did_ins_2 [first_last_ts.length];
			for (int n = 0; n < first_last_ts.length; n++) {
				String ts= first_last_ts[n];
				xyz_fl[n] = ers_reference.getSceneXYZ(ts);
				did_2_fl[n] = scene_map.get(ts).did_ins_2;
			}
			double travel_visual2 = 0;
			double travel_ims2 = 0;
			double [] dned = Imx5.nedFromLla (did_2_fl[1].lla, did_2_fl[0].lla);
			for (int i = 0; i < 3; i++) {
				double dv = xyz_fl[1][i]-xyz_fl[0][i];
				travel_visual2 += dv*dv;
				travel_ims2 += dned[i]*dned[i];
			}
			dist_visual[nref] = Math.sqrt(travel_visual2);
			dist_ims2[nref] =   Math.sqrt(travel_ims2);
			double img_scale = Math.sqrt(travel_visual2/travel_ims2);
			img_scales[nref] =  img_scale;
			double disp_corr = 	getImsDisparityCorrection(
					img_scale,   // double           scale_img,
					ref_scene,   // QuadCLT          ref_CLT,
					use_lma_dsi, // boolean          use_lma_dsi,
					debugLevel); // final int        debugLevel) {
			disp_corrs[nref] = disp_corr;
		}
		double sw = 0.0, swd = 0.0; // , sw_vis=0, sw_ims=0;
		for (int nref = 0; nref < refCLTs.length; nref++) {
			double w = 1.0;
			sw += w;
			swd += w * disp_corrs[nref];
//			sw_vis += dist_visual[nref];
//			sw_ims += dist_ims2[nref];
		}
		double disp_corr = swd/sw;
		if (infinity_rslt != null) {
			for (int nref = 0; nref < refCLTs.length; nref++) {
				infinity_rslt[nref] = new double[] {disp_corrs[nref], img_scales[nref], dist_visual[nref], dist_ims2[nref]};
			}			
		}
		/*
		if (debugLevel > -3) {
			System.out.println("ts\timg_scale\tdist_visual\tdist_ims");
			for (int nref = 0; nref < refCLTs.length; nref++) {
				System.out.println(refCLTs[nref].getImageName()+"\t"+img_scales[nref]+"\t"+dist_visual[nref]+"\t"+dist_ims2[nref]);
			}			
			System.out.println("Average\t"+img_scale+"\t"+(sw_vis/sw)+"\t"+(sw_ims/sw));
		}
		*/
    	return disp_corr;
    }
	
	public static double getImsDisparityCorrection(
			double           scale_img,
			QuadCLT          ref_CLT,
			boolean          use_lma_dsi,
			final int        debugLevel) {
		boolean dbg=debugLevel > 1000;
		// ref_scene.restoreAnyDSI(debugLevel);
		double [][] dls = ref_CLT.getDLS();
		if (dls == null) {
			ref_CLT.restoreAnyDSI(debugLevel);
			dls = ref_CLT.getDLS();
			if (dls == null) {
				System.out.println("*** BUG: getImsDisparityCorrection(): dsi==null and no dsi file is available");
				return Double.NaN;
			}
		}
		double [] disparity_dsi = dls[use_lma_dsi?1:0]; // [1]; // null
		double [] strength = ref_CLT.getDLS()[2];
		double sw=0,swd=0; // old, new-old
		int num_good = 0;
		if (dbg) {
			ShowDoubleFloatArrays.showArrays(
					new double[][] {disparity_dsi, strength},
					ref_CLT.getTilesX(),
					ref_CLT.getTilesY(),
					true,
					ref_CLT.getImageName()+"getImsDisparityCorrection",
					new String[] {"disparity","strength"}); //	dsrbg_titles);
		}

		for (int i = 0; i < disparity_dsi.length; i++) {
			if (!Double.isNaN(disparity_dsi[i])) {
				if (Double.isNaN(strength[i])) {
					System.out.println("getImsDisparityCorrection(): strength["+i+"]=NaN");
				} else {
					sw   += strength[i];
					swd  += strength[i]*disparity_dsi[i];
					num_good++;
				}
			}
		}
		double disp_avg = swd/sw;
		if (debugLevel > -3) {
			System.out.println("getImsDisparityCorrection() sw ="+sw+" swd="+swd+" disp_avg="+disp_avg+
					" num_good="+num_good+" in reference scene "+ref_CLT.getImageName());
		}

		double inf_disp_ref = disp_avg * (1.0 - scale_img);
		if (debugLevel > -3) {
			System.out.println("Disparity at infinity ="+inf_disp_ref+" in reference scene "+ref_CLT.getImageName());
		}
		return inf_disp_ref;
	}

	
	public static void saveInfinitySeries(
//    		CLTParameters    clt_parameters,
    		QuadCLT []       refCLTs,
    		QuadCLT          index_CLT, // normally - just the last in quadCLTs
    		double [][]      infinity_rslt,
    		int              debugLevel) {
		String header = "ts\tinfinity\timg_scale\tdist_visual\tdist_ims\n";
		StringBuffer sb = new StringBuffer();
 		double sw = 0.0, swd = 0.0, sw_scale = 0, sw_vis=0, sw_ims=0;
		for (int nref = 0; nref < refCLTs.length; nref++) {
			double w = 1.0;
			sw += w;
			swd += w *   infinity_rslt[nref][0]; // disparity
			sw_scale +=  infinity_rslt[nref][1];
			sw_vis +=    infinity_rslt[nref][2];
			sw_ims +=    infinity_rslt[nref][3];
			sb.append(refCLTs[nref].getImageName()+"\t"+infinity_rslt[nref][0]+"\t"+infinity_rslt[nref][1]+"\t"+infinity_rslt[nref][2]+"\t"+infinity_rslt[nref][3]+"\n");
		}
		sb.append("Average\t"+(swd/sw)+"\t"+(sw_scale/sw)+"\t"+(sw_vis/sw)+"\t"+(sw_ims/sw)+"\n");
	    Path inf_path = Paths.get(index_CLT.getX3dDirectory(true)).resolve(index_CLT.getImageName()+SUFFIX_SERIES_INFINITY);
		CalibrationFileManagement.saveStringToFile (
				inf_path.toString(),
				header+sb.toString());
		if (debugLevel > -3) {
			System.out.println("Infinity correction data saved to "+inf_path.toString());
		}
		return;
	}
	
    public static double[] adjustImuOrient(
    		CLTParameters clt_parameters,     // CLTParameters clt_parameters,
    		boolean       orient_combo,
    		QuadCLT[][]   quadCLTss,
    		int []        ref_indices,
    		QuadCLT       index_scene, // where to put result
    		boolean       save_details,
    		int           debugLevel) {
		boolean orient_by_move =    clt_parameters.imp.orient_by_move; // use translation data to adjust IMU orientation
		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
		
		double  transl_cost =       clt_parameters.imp.wser_orient_transl; // AGL corrected, 0.5 - equal weights, 0.0 - rotation only, 1.0 - translation-only
		
		
		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 quat_lma_mode = orient_combo?QuaternionLma.MODE_COMBO_LOCAL: QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; //  2; // 1;
    	int debug_lev = debugLevel; // 3;
    	double  translation_agl =  1.0 / (avg_z + 1.0);
    	if (!orient_by_move) {
    		translation_agl = 0.0;
    	} else if (!orient_by_rot) {
    		translation_agl = 1.0;
    	}
    	// Apply transl_cost
    	double translation_weight = (translation_agl * transl_cost) / (translation_agl * transl_cost + (1-translation_agl)* (1-transl_cost));
    	
    	// boolean       orient_combo,
    	// boolean       save_details
    	if (debugLevel > -3) {
    		System.out.println("adjustImuOrient(): transl_cost="+transl_cost+", translation_agl="+translation_agl+", translation_weight="+translation_weight);
    	}
    	String        suffix = save_details ? (orient_combo ? QuadCLTCPU.IMU_CALIB_COMBO_SUFFIX: QuadCLTCPU.IMU_CALIB_DETAILS_SUFFIX) : null;
    	double [][][][] rotated_xyzatrs = rotateImsToCameraXYZ(
    			clt_parameters,     // CLTParameters clt_parameters,
    			quat_lma_mode,      // int quat_lma_mode,
    			avg_z,              // double        avg_height,
    			translation_weight, // double        translation_weight,
    			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]
    			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)
        	/*
        	 saveStringInModelDirectory(
			sb.tiString, // String  string,
			IMU_CALIB_LOGS_SUFFIX); // String  suffix)
        	 */
        	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("getNumOrient()="+quadCLTs[ref_index].getNumOrient()+" of "+clt_parameters.imp.min_num_orient+"\n");
        	/// sb.append("getNumAccum()= "+quadCLTs[ref_index].getNumAccum()+ " of "+clt_parameters.imp.min_num_interscene+"\n");
        	sb.append("avg_z= "+avg_z+" m\n");
        	sb.append("translation_weight= "+translation_weight+"\n");
        	sb.append("quat_lma_mode= "+quat_lma_mode+"\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 [][][][] rotateImsToCameraXYZ(
    		CLTParameters clt_parameters,
        	int           quat_lma_mode,
        	double        avg_height,
        	double        translation_weight,  // 1.0 - pure translation, 0.0 - pure rotation
    		QuadCLT[][]     quadCLTss,
    		double [][][][] xyzatrs,
    		double [][][][] ims_xyzatrs,
    		int []          ref_indices,
			double []     rms, // null or double[5];
			double []     quaternion, // null or double[4]
			QuadCLT       index_scene, // where to put result
			String        suffix,
			int           debugLevel) {
    	final boolean use3 = false; // true; // false; // (quat_lma_mode == 3); // true;// extract from clt ?
    	boolean use_inv = false; //
    	double [] quat = getRotationFromXYZATRCameraIms(
    			clt_parameters,     // CLTParameters   clt_parameters,
    			quat_lma_mode,      // int             quat_lma_mode,
    			use3,               // boolean         use3, // false - full4 quaternion (with scale), true - only q1,q2,q3
    			avg_height,         // double          avg_height,    			
    			translation_weight, // double          translation_weight,
    			quadCLTss,          // QuadCLT[][]     quadCLTss,
    			xyzatrs,            // double [][][][] xyzatrs,
    			ims_xyzatrs,        // double [][][][] ims_xyzatrs,
    			rms,                // double []     rms, // null or double[5];
    			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
    	int num_refs = ims_xyzatrs.length;
    	double [][][][] rotated_xyzatrs = new double [num_refs][][][];
    	for (int nref = 0; nref < num_refs; nref++) {
    		int num_scenes = ims_xyzatrs[nref].length;
    		rotated_xyzatrs[nref] = new double [num_scenes][][]; // orientation from camera, xyz from rotated IMS
    		double [] rotated_xyz = new double[3];
    		Rotation rot_invert = rot.revert();
    		for (int nscene = 0; nscene < num_scenes; nscene++) {
    			rot.applyTo(ims_xyzatrs[nref][nscene][0], rotated_xyz);
    			// ???
    			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]);
    			Rotation ims_atr = 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 rotation_atr,rotation_atr2;
    			if (use_inv) { // should not be used
    				rotation_atr = rot_invert.applyTo(ims_atr);
    				rotation_atr2 = rotation_atr.applyTo(rot); // applyInverseTo?
    			} else {
    				rotation_atr = rot.applyTo(ims_atr);
    				rotation_atr2 = rotation_atr.applyTo(rot_invert); // applyInverseTo?
    			}
    			rotated_xyzatrs[nref][nscene] = new double [][] {rotated_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,
        			rotated_xyzatrs,          // double [][][] rotated_xyzatr,
        			orient_path.toString()); // String        path) {
    		if (debugLevel > -3) {
    			System.out.println("Orientation details saved to "+orient_path.toString());
    		}
        }
    	return rotated_xyzatrs;
    }
      
    public static void saveRotateImsMultiDetails(
    		Rotation        rot,
    		double [][][][] xyzatrs,
    		double [][][][] ims_xyzatrs,
    		double [][][][] rotated_xyzatrs,
    		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(
				"N\tX\tY\tZ\tA\tT\tR\t"+
				"PIMU-X\tPIMU-Y\tPIMU-Z\tPIMU-A\tPIMU-T\tPIMU-R\t"+
				"ROT-X\tROT-Y\tROT-Z\tROT-A\tROT-T\tROT-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());
    }
      
    
    
    
    /**
     * Refine scene poses (now only linear) from currently adjusted poses
     * (adjusted using IMS orientations) and IMS positions (currently
     * only linear are used). Refined scene poses may be used as pull targets
     * with free orientations (and angular velocities for ERS).
     * Result poses are {0,0,0} for the reference frame.
     * 
     * Assuming correct IMU angular velocities and offset linear ones.   
     * 
     * @param clt_parameters configuration parameters
     * @param quadCLTs       scenes sequence (for timestamps)
     * @param xyzatr         current scenes [[x,y,z],[a,t,r]] in reference scene frame
     * @param ims_xyzatr     IMU-derived (integrated) scene position and orientation in reference
     *                       scene frame (only position is currently used)
     * @param ref_index      reference scene index 
     * @param early_index    earliest (lowest) scene index to use
     * @param last_index     last (highest) scene index to use
     * @return quaternion    componets 
     */
    
    public static double [] getRotationFromXYZATRCameraIms(
    		CLTParameters   clt_parameters,
    		int             quat_lma_mode,
    		boolean         use3, // false - full4 quaternion (with scale), true - only q1,q2,q3
    		double          avg_height,
    		double          translation_weight,
    		QuadCLT[][]     quadCLTss,
    		double [][][][] xyzatrs,
    		double [][][][] ims_xyzatrs,
			double []       rms, // null or double[5];
			int             debugLevel) {
    	boolean use_scale_y = false;
//    	final boolean use3 = (quat_lma_mode == 3); // true;
		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;
		
		int num_samples = 0;
		for (int nref = 0; nref < xyzatrs.length;nref++) {
			num_samples += xyzatrs[nref].length;
		}
		
		double []   quat0 = use3? new double[3] : new double [] {1.0, 0.0, 0.0, 0.0}; // identity
		QuaternionLma quaternionLma = new QuaternionLma();
//		double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR
//		double [][][] vect_x = new double [quadCLTs.length][][]; // IMS XYZATR
		double [][][] vect_y = new double [num_samples][][]; // camera XYZATR
		double [][][] vect_x = new double [num_samples][][]; // IMS XYZATR
		int vindx = 0;
		for (int nref = 0; nref < xyzatrs.length;nref++) {
			for (int nscene = 0; nscene <  xyzatrs[nref].length; nscene++) {
				vect_x[vindx] =   ims_xyzatrs[nref][nscene];
				vect_y[vindx++] = xyzatrs[nref][nscene];
			}
		}
		// Is it needed now (below)? It scalesvect_y to have the same length as vect_x
		double [][][] vect_y_scaled;
		if (use_scale_y) {
			vect_y_scaled= QuaternionLma.scaleXYZ(
					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
		} else {
			vect_y_scaled = vect_y;
		}
		if ((quat_lma_mode == QuaternionLma.MODE_COMBO) || (quat_lma_mode == QuaternionLma.MODE_COMBO_LOCAL)) {
			quaternionLma.prepareLMA(
					quat_lma_mode,      // int           mode,
					avg_height,         // double        avg_height,
					vect_x,             // double [][][] vect_x,
					vect_y_scaled,      // vect_y,             // double [][][] vect_y,
					null,               // double []     vect_w, all same weight
					0.0, // reg_w,              // double        reg_w,      // regularization weight [0..1)  
					quat0,              // double []     quat0,
					debugLevel);        // int           debug_level)
			double [] mn_mx_diag = quaternionLma.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/quat0.length);
				if (debugLevel > -3) {
					System.out.println("====1 Calculated reg_w = "+reg_w+", reg_w_old="+reg_w_old+", a="+a);
				}
				quaternionLma.prepareLMA(
						quat_lma_mode,      // int           mode,
						avg_height,         // double        avg_height,
						vect_x,             // double [][][] vect_x,
						vect_y_scaled,      // vect_y,             // double [][][] vect_y,
						null,               // double []     vect_w, all same weight
						reg_w,              // double        reg_w,      // regularization weight [0..1)  
						quat0,              // double []     quat0,
						debugLevel);        // int           debug_level)
			}			
			
		} else if ((quat_lma_mode == QuaternionLma.MODE_XYZQ) ||
				(quat_lma_mode == QuaternionLma.MODE_XYZQ_LOCAL) ||
				(quat_lma_mode == QuaternionLma.MODE_XYZ4Q3)) {
			quaternionLma.prepareLMA(
					quat_lma_mode,      // int           mode,
					vect_x,             // double [][][] vect_x,
					vect_y_scaled, // vect_y,             // double [][][] vect_y,
					null,               // double []     vect_w, all same weight
					translation_weight, // double        translation_weight, // 0.0 ... 1.0;
					0.0, // reg_w,              // double        reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  
					quat0,              // double []     quat0,
					debugLevel);        // int           debug_level)
			double [] mn_mx_diag = quaternionLma.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/quat0.length);
				if (debugLevel > -3) {
					System.out.println("====2 Calculated reg_w = "+reg_w+", reg_w_old="+reg_w_old+", a="+a);
				}
				quaternionLma.prepareLMA(
						quat_lma_mode,      // int           mode,
						vect_x,             // double [][][] vect_x,
						vect_y_scaled, // vect_y,             // double [][][] vect_y,
						null,               // double []     vect_w, all same weight
						translation_weight, // double        translation_weight, // 0.0 ... 1.0;
						reg_w,              // double        reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  
						quat0,              // double []     quat0,
						debugLevel);        // int           debug_level)
			}
		} else {
			quaternionLma.prepareLMA(
					vect_x,        // quat_lma_xyz,     // double [][] vect_x,
					vect_y_scaled, // vect_y,             // double [][][] vect_y,
					null,          // double [][] vect_w, all same weight
					reg_w,         // double      reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  
					quat0,         // double []   quat0,
					debugLevel);   // int   debug_level)
		}
		
		int lma_result = quaternionLma.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 (rms != null) { // null or double[2];
				double [] last_rms = quaternionLma.getLastRms();
				rms[0] = last_rms[0];
				rms[1] = last_rms[1];
				if (rms.length >= 4) {
					double [] initial_rms = quaternionLma.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 quaternionLma.getQuaternion();
		}
    }
	
	/*
		SetChannels [] set_channels=quadCLT_main.setChannels(debugLevel);
		QuadCLT [] quadCLTs = new QuadCLT [set_channels.length]; 
		for (int i = 0; i < quadCLTs.length; i++) {
			quadCLTs[i] = (QuadCLT) quadCLT_main.spawnQuadCLT(
					set_channels[i].set_name,
					clt_parameters,
					colorProcParameters, //
					threadsMax,
					debugLevel);
			quadCLTs[i].setDSRBG(
					clt_parameters, // CLTParameters  clt_parameters,
					threadsMax,     // int            threadsMax,  // maximal number of threads to launch
					updateStatus,   // boolean        updateStatus,
					debugLevel);    // int            debugLevel)
		}
		if (ref_index < 0) {
			ref_index += quadCLTs.length;
		}
			
	 * 
	 * 
   public static double getImgImsScale( // correctInfinityFromIMS(
			QuadCLT []       quadCLTs,
			QuadCLT          master_CLT,
			int              earliest_scene,
			int              latest_scene) { // -1 will use quadCLTs.length -1;

	 */
	
}
