/**
 ** OrthoAltitudeMatch - Represent elevation maps matching for pair of scenes 
 **
 ** Copyright (C) 2024 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  PairwiseOrthoMatch.java is free software: you can redistribute it and/or modify
 **  it under the terms of the GNU General Public License as published by
 **  the Free Software Foundation, either version 3 of the License, or
 **  (at your option) any later version.
 **
 **  This program is distributed in the hope that it will be useful,
 **  but WITHOUT ANY WARRANTY; without even the implied warranty of
 **  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 **  GNU General Public License for more details.
 **
 **  You should have received a copy of the GNU General Public License
 **  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 ** -----------------------------------------------------------------------------**
 **
 */

package com.elphel.imagej.orthomosaic;

import java.awt.Rectangle;
import java.io.IOException;
import java.text.SimpleDateFormat;
import java.util.Arrays;
import java.util.Calendar;

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

import Jama.Matrix;
import ij.IJ;

public class OrthoAltitudeMatch {

	public static boolean altutudeMatchPairs(
			CLTParameters       clt_parameters,
			OrthoMapsCollection orthoMapsCollection, 
			int [][]            available_pairs, 
			boolean             alt_overwrite, // overwrite existing altitude match pairs
			boolean             alt_pairwise,  // use pairwise affines if available
			double              alt_sigma,     // 5.0; Reduce weight of the border tiles, Gaussian sigma in tiles to apply to weights.
			double              alt_abs_outliers, //  =   3.0;     // remove absolute outliers when fitting planes 
			double              alt_outliers,  // 0.05; Remove outliers when fitting planes, removed fraction.
			int                 alt_refine,    // 1; Refine altitude difference plane after removing outliers (0 - no outlier removal, 1 - remove outliers and refine once, ...)
			double              metric_error,
			double              weight_rot,    // >0 weight of pairs errors in qn3
			double              weight_tilt,   // >0 weight of pairs errors in qn1, qn2
			double              weight_scale,  // >0 weight in pairs scale-1.0 errors
			double              pull,          // 0 <= pull <1 - fraction of all RMS contributors
			double              pull_rots,     // >=0 weight of sum of rotations, may be 0, normalized by pull value 
			double              pull_tilts,    // >=0 weights of sum of qn1 and qn2 of scenes, normalized by pull value
			double              pull_scales,   // >=0 weights of scales of scenes, normalized by pull value
			// log/save parameters
			boolean             save_each,
			boolean             log_append,
			String              log_path,
			String              orthoMapsCollection_path,
			int                 debugLevel) {
        boolean dbg_planes = false;
		boolean y_down_ccw = true;
		boolean invert_q2a = false;
		boolean test_quat =  false;
		boolean test_quat0 = false;
		boolean test_quat2=  false; // true;
        boolean use_degrees = true;
        boolean use_percents = true;
        
        boolean debug_tilts = true;
        
		boolean invert_order= false; // print them
		boolean invert_y =    true; // only for tilts->affines
        boolean show_details =false;
        
        boolean do_not_save = false;

//        OrientationSceneLMA.testGetPairErrQuaternion ();
//        OrientationSceneLMA.testGetPairPairScaleDirError();
        
		if (test_quat0) {
			QuatUtils.testQuatAff();
		}
		
		
		int [] indices = orthoMapsCollection.getScenesFromPairs( // may be shorter, each element - absolute scene number used in pairs
				available_pairs,  // pairs_defined_abs,// int [][] pairs,
				null);            // int [] indices_in)  // preselected indices or null
		boolean updateStatus = true;
		int [][] condensed_pairs = orthoMapsCollection.condensePairs (available_pairs, indices);
		int alt_zoom_offs = -3;   // altitude resolution is 3 steps lower than the images
		if (indices.length < 2) {
			System.out.println("too few scenes remain: "+indices.length);
			return false;
		}
		OrthoMap[] ortho_maps = orthoMapsCollection.getMaps();
		int min_zoom_all = ortho_maps[indices[0]].getOriginalZoomLevel();
		int max_zoom_all = ortho_maps[indices[0]].getOriginalZoomLevel();
		for (int i = 1; i < indices.length; i++) {
			min_zoom_all = Math.min(min_zoom_all, ortho_maps[indices[i]].getOriginalZoomLevel());
			max_zoom_all = Math.max(max_zoom_all, ortho_maps[indices[i]].getOriginalZoomLevel());
		}
		int alt_zoom_lev = Math.min(max_zoom_all+alt_zoom_offs, min_zoom_all);
		double pix_size_meters = OrthoMap.getPixelSizeMeters(alt_zoom_lev);
		if (debugLevel > 0 ) {
			System.out.println("max_zoom="+max_zoom_all+ " min_zoom="+min_zoom_all+" alt_zoom_lev="+alt_zoom_lev);
		}
		int min_scene =      0; // use if needs to continue (during development)
		if (log_append && (log_path != null)) { // assuming directory exists
			StringBuffer sb = new StringBuffer();
			sb.append(new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(Calendar.getInstance().getTime())+"\n");
			
			sb.append("number of scenes pairs\t"+     available_pairs.length+"\n");
			sb.append("number of scenes\t"+     indices.length+"\n");
			sb.append("minimal zoom level\t"+   min_zoom_all+"\n");
			sb.append("maximal zoom level\t"+   max_zoom_all+"\n");
			sb.append("altitude zoom level\t"+  alt_zoom_lev+"\n");
			
//			sb.append(String.format("%4s\t%4s\t%17s\t%17s\t%6s\t%3s\t%6s\t%6s\t%7s\n",
//					"scn1","scn2","timestamp1","timestamp2","ovrlp","zl","RMS-sp","RMSfin","fzl","removed"));
			sb.append(String.format("%4s\t%4s\t%17s\t%17s\t%6s\t%3s\t%6s\t%7s\t%7s\t%7s\n",
					"scn1","scn2","timestamp1","timestamp2","ovrlp","zl","RMS","tiltX","tiltY","offs"));
			CalibrationFileManagement.saveStringToFile (
					log_path,          //String path,
					sb.toString(), // data,
					true); // boolean append)
			if (debugLevel>-3) {
				System.out.print(sb.toString());
			}
		}
		// create altitude map
		int [] wh =     new int[2];
		int [] origin = new int[2];
		boolean show_centers = true;
		boolean  bounds_to_indices = true;
		double [][] centers = show_centers? (new double [indices.length][]): null;
		double [][] alt_multi = orthoMapsCollection.renderMultiDouble (
				new double [indices.length][] , // double [][]   ground_planes, // null - images, non-null altitudes. use new double[2][3] for old way alt
				indices,                        // int []        indices, // null or which indices to use (normally just 2 for pairwise comparison)
				bounds_to_indices,              // boolean       bounds_to_indices,
				null,                           // affines,    // double [][][] affines, // null or [indices.length][2][3]
				null,                           // double [][]   equalize,
				true,                           // boolean       ignore_equalize,
				null,                           // warp,       // FineXYCorr    warp,,
				alt_zoom_lev,                       // int         zoom_level,
				wh,                             // int []      wh,
				origin,                         // int []      origin){ // maps[0] as a reference
				centers);                       // double [][] centers)
		final int width = wh[0];
//		final int height = wh[1];
		final int num_scenes = indices.length;
		final int num_pairs = condensed_pairs.length;
//		int num_pairs = 0; // available_pairs.length
//		ArrayList<Point> failed_pairs = new ArrayList<Point>();
		double [] weights_scenes = new double [num_scenes];
		double [] weights_pairs = new double [num_pairs];
		Arrays.fill(weights_scenes, 1.0);
		Arrays.fill(weights_pairs, 1.0);
		double [][][] scene_tilts_pairs = new double [num_pairs][2][];
		double [][]   quat_pairs = new double [num_pairs][]; // pairwise quaternions combining 2 axes from tilts, rotation and scale - from affine  
		double [][][] affine_pairs = new double [num_pairs][][];
		double [] flat_err = new double[num_pairs];
		double [] overlaps = new double [num_pairs];
		
		double tilt_err_threshold = 0.01; // reduce weight if larger
		double overlop_pow =        2.0; // squared
		double weight_pairs_k =     100.0;
		for (int npair = 0; npair < condensed_pairs.length; npair++) {
			int [] cpair = condensed_pairs[npair]; // index alt_multi
			int [] ipair = {indices[cpair[0]], indices[cpair[1]]};
			if (updateStatus) {
				IJ.showStatus("Processing pair "+npair+" of "+condensed_pairs.length+" "+ipair[0]+"->"+ipair[1]);
				IJ.showProgress(1.0*npair/(condensed_pairs.length-1));
			}
			PairwiseOrthoMatch pairwiseOrthoMatch = ortho_maps[ipair[0]].getMatch(ortho_maps[ipair[1]].getName(), true); // ?
    		if (pairwiseOrthoMatch == null) {
    			System.out.println("BUG: Missing pair for ["+ipair[0]+", "+ipair[1]+"] ");
    			continue;
    		}
			if (ipair[0] < min_scene) {
				System.out.println ("Skipping "+ipair[0]+":"+ipair[1]+" until "+min_scene);
				continue;
			}
			if ((pairwiseOrthoMatch.getAltData() != null) && !alt_overwrite)  {
				System.out.println ("Skipping "+ipair[0]+":"+ipair[1]+" as it has alt_data defined and alt_overwrite == false .");
				continue;
			}
			// pairwiseOrthoMatch is clone() - not anymore
			
	    	int min_zoom_lev = ortho_maps[ipair[0]].getOriginalZoomLevel();
	    	int max_zoom_lev = ortho_maps[ipair[0]].getOriginalZoomLevel();
	    	double max_agl =   ortho_maps[ipair[0]].getAGL();
	    	for (int i = 0; i < ipair.length; i++) {
	    		max_agl =      Math.max(max_agl, ortho_maps[ipair[i]].getAGL());	    		
	    		min_zoom_lev = Math.min(min_zoom_lev, ortho_maps[ipair[i]].getOriginalZoomLevel());
	    		max_zoom_lev = Math.max(max_zoom_lev, ortho_maps[ipair[i]].getOriginalZoomLevel());
	    	}
	    	double [][] alt_slices = {alt_multi[cpair[0]], alt_multi[cpair[1]]};
	    	Rectangle woi_overlap = OrthoMap.getDefinedBounds(
	    			alt_slices, // final double [][] data_slices,
	    			width); // final int width);
	    	double [] diff_data = OrthoMap.subtractWoi(
	    			alt_multi[cpair[0]], // final double [] data0,
	    			alt_multi[cpair[1]], // final double [] data1,
	    			width,               // final int       width,
	    			woi_overlap);        // Rectangle woi_in);
	    	
	        double []  xy0 = {0.5*woi_overlap.width, 0.5*woi_overlap.height};
	        double [] weight = null; //  make it fading?
	        if (alt_sigma > 0) {
	        	weight = OrthoMap.getBorderWeights(
	        			diff_data, //final double []  data,
	        			alt_sigma, // final double     sigma,
	        			woi_overlap.width);    // int              width)
	        }
	        
	        boolean [] mask = null;
	        double [] alt_data5 = null;
        	int    num_bins = 1000;
    		for (int ntry = 0; ntry <= alt_refine; ntry++) {
    			String dbg_name = (dbg_planes && (ntry == alt_refine))  ? ("plane_approximation_"+ipair[1]+"-"+ipair[0]+"_"+ntry) :null;
//    			String dbg_name = (dbg_planes )  ? ("plane_approximation_"+ipair[1]+"-"+ipair[0]+"_"+ntry) :null;
    			alt_data5 = OrthoMap.getPlane(
    	        		diff_data,         // final double []   data,
    	    			mask,              // final boolean [] mask,
    	    			weight,            // final double []  weight,
    	    			woi_overlap.width, // final int        width,
    	    			xy0,              // final double []  xy0) {
    	    			dbg_name);
    	        if ((alt_outliers > 0) && (ntry < alt_refine)){ // not the last pass
	    			mask = OrthoMap.removeRelativeLowHigh (
	    					diff_data, //  final double [] data,
	    					null, // mask, // final boolean [] mask_in, // new mask for all data and latest plane
	    					alt_abs_outliers, // final double abs_diff,
	    					alt_outliers,     // final double rel_frac,
	    					alt_data5,        // final double []  ground_plane, // tiltx,tilty, offs, x0(pix), y0(pix) or null
	    					woi_overlap.width,            // final int     width, // only used with ground_plane != null;
	    					num_bins);        // final int    num_bins)	    			
    	        } else {
    	        	break;
    	        }
    		}
	        double [] alt_data = {alt_data5[0]/pix_size_meters, alt_data5[1]/pix_size_meters,alt_data5[2]};
        	double [][] alt_datas = new double[3][];
	        boolean [][] masks = new boolean[2][];
	        double [][] alt_data5s = new double[2][];
        	double [][] data_overlap = new double[2][];
	        
        	// calculate individual tilts
	        for (int ns = 0; ns < cpair.length; ns++) {
	        	data_overlap[ns] = OrthoMap.extractWoi(
	        			alt_multi[cpair[ns]], // final double [] data0,
		    			width,                // final int       width,
		    			woi_overlap);         // Rectangle woi_in);
	        	
	    		for (int ntry = 0; ntry <= alt_refine; ntry++) {
	       			String dbg_name = (dbg_planes && (ntry == alt_refine))  ? ("plane_approximation_"+ipair[ns]+"_"+npair) :null;
	    			alt_data5s[ns] = OrthoMap.getPlane(
	    					data_overlap[ns],  // final double []   data,
	    					masks[ns],         // final boolean [] mask,
	    	    			weight,            // final double []  weight,
	    	    			woi_overlap.width, // final int        width,
	    	    			xy0,              // final double []  xy0) {
	    	    			dbg_name);
	    	        if ((alt_outliers > 0) && (ntry < alt_refine)){ // not the last pass
	    	        	masks[ns] = OrthoMap.removeRelativeLowHigh (
	    	        			data_overlap[ns], //  final double [] data,
		    					null,    // mask, // final boolean [] mask_in, // new mask for all data and latest plane
		    					alt_abs_outliers, // final double abs_diff,
		    					alt_outliers,     // final double rel_frac,
		    					alt_data5s[ns],   // final double []  ground_plane, // tiltx,tilty, offs, x0(pix), y0(pix) or null
		    					woi_overlap.width,// final int     width, // only used with ground_plane != null;
		    					num_bins);        // final int    num_bins)	    			
	    	        } else {
	    	        	break;
	    	        }
	    		}
	    		alt_datas[ns] = new double [] {alt_data5s[ns][0]/pix_size_meters, alt_data5s[ns][1]/pix_size_meters,alt_data5s[ns][2]};
	        }
	        double tilt_err2 = 0;
	        alt_datas[2] = new double [3];
	        for (int i = 0; i < alt_datas[2].length; i++) {
	        	alt_datas[2][i] = alt_datas[0][i]+alt_data[i]; // simulated alt_datas[1]
	        	if (i < 2) {
	        		double dt = alt_datas[2][i]-alt_datas[1][i];
	        		tilt_err2+=dt*dt; 
	        	}
	        }
	        scene_tilts_pairs[npair] = new double [][] {alt_datas[0], alt_datas[2]}; // {tilt0, tilt1 (synthetic)
	        affine_pairs[npair] = pairwiseOrthoMatch.getAffine(); // contains scale + rot
	        flat_err[npair] = Math.sqrt(tilt_err2);
	        overlaps[npair] = pairwiseOrthoMatch.getOverlap();
	        quat_pairs[npair] = QuatUtils.sceneRelLocalGround(
        			alt_data, // double []   txy, differential tilts
        			affine_pairs[npair], // double [][] affine,
        			y_down_ccw); // boolean y_down_ccw)
	        if (!test_quat2) {
	        	continue;
	        }
        	
        	
			if (test_quat2) {
	        	System.out.println("***************** npair="+npair+": "+ipair[0]+" -> "+ipair[1]+
	        			", invert_q2a="+invert_q2a+", invert_order="+invert_order+", invert_y="+invert_y);
		        double [][] affine_pair = pairwiseOrthoMatch.getAffine();
		        boolean remove_rs = false;
		        if (remove_rs) {
		        	affine_pair= SingularValueDecomposition.removeTiltRotScale(
		        			affine_pair, // double [][] A,
		        			false, // boolean removeTilt,
		        			true,  // boolean removeRot,
		        			true,  // boolean removeScale,
		        			false, // boolean removeOffset,
		        			false); // boolean max_is_scale);
		        } else {
		        	affine_pair= new double [][] {affine_pair[0].clone(),affine_pair[1].clone()};  
		        }
		        double [][] affine_pair_nors = SingularValueDecomposition.removeTiltRotScale(
		        		affine_pair, // double [][] A,
		    			false, // boolean removeTilt,
		    			true,  // boolean removeRot,
		    			true,  // boolean removeScale,
		    			false, // boolean removeOffset,
		    			false); // boolean max_is_scale);
		        
		        
		        // calculate second from first and pair
		        double [][] affine0 = ortho_maps[ipair[0]].getAffine();
				Matrix A0 = new Matrix (
						new double [][] {{affine0[0][0],affine0[0][1]},{affine0[1][0],affine0[1][1]}});
				Matrix A10 = new Matrix (
						new double [][] {{affine_pair[0][0],affine_pair[0][1]},{affine_pair[1][0],affine_pair[1][1]}});
		        Matrix A1a = A10.times(A0);
		        double [][] affine1a = new double[][] {
					{A1a.get(0,0),A1a.get(0,1)},
					{A1a.get(1,0),A1a.get(1,1)}};
		        double [][][] affines  = new double [][][] {ortho_maps[ipair[0]].getAffine(),ortho_maps[ipair[1]].getAffine(), affine1a};
		        
		        SingularValueDecomposition svd_affine_pair =  SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_pair, y_down_ccw); // boolean y_down_ccw)
		        SingularValueDecomposition[] svd_affines =    {
		        		SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[0], y_down_ccw),
		        		SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[1], y_down_ccw),
		        		SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[2], y_down_ccw)};
	        	
		        
//		        alt_datas[0] = alt_datas[0].clone();
		        double [] tilts0_mod =  QuatUtils.manualFitTilt(
		        		alt_datas[0], // double [] tilts0,
		        		alt_data,     // double [] tilts_diff,
		        		affine_pair_nors, // double [][] affine_pair_nors,
		        		invert_q2a, // boolean invert_q2a) // invert result affines (to match "usual")
//	        			debug_tilts,
	        			invert_order, // boolean invert_order,
	        			invert_y); // boolean invert_y)
		        if (!show_details) {
		        	continue;
		        }
		        
		        alt_datas[0]=tilts0_mod.clone();
		        
		        alt_datas[2] = new double [3];
		        for (int i = 0; i < alt_datas[2].length; i++) {
		        	alt_datas[2][i] = alt_datas[0][i]+alt_data[i]; // simulated alt_datas[1] 
		        }
		        
	        	System.out.println(QuatUtils.affinesToString(affine_pair, "affine_pair  "));
	        	System.out.println("svd_affine_pair= " + svd_affine_pair.toString(use_degrees));
		        System.out.println();
	        
	        	System.out.println(QuatUtils.affinesToString(affines[0] , "affines[0]=  "));
		        System.out.println("svd_affines[0]=  " + svd_affines[0].toString(use_degrees));
		        System.out.println();
		        
	        	System.out.println(QuatUtils.affinesToString(affines[1] , "affines[1]=  "));
		        System.out.println("svd_affines[1]=  " + svd_affines[1].toString(use_degrees));
		        System.out.println();
		        
	        	System.out.println(QuatUtils.affinesToString(affines[2] , "affines[2]=  "));
		        System.out.println("svd_affines[2]=  " + svd_affines[2].toString(use_degrees));
		        System.out.println();
		        
		        System.out.println("tilt_diff"+  QuatUtils.tiltToString (alt_data, y_down_ccw, use_degrees));
		        System.out.println("tilt[0]  "+  QuatUtils.tiltToString (alt_datas[0],       y_down_ccw, use_degrees));
		        System.out.println("tilt[1]  "+  QuatUtils.tiltToString (alt_datas[1], y_down_ccw, use_degrees));
		        System.out.println("tilt[2]  "+  QuatUtils.tiltToString (alt_datas[2], y_down_ccw, use_degrees));
		        System.out.println();
		        
		        double [][] quat_scenes = new double [alt_datas.length][];
	        	double [] quat_diff_scenes = QuatUtils.sceneRelLocalGround(
	        			alt_data, // double []   txy,
	        			affine_pair, // double [][] affine,
	        			y_down_ccw); // boolean y_down_ccw)
		        System.out.println("quat_diff_scenes=  "+QuatUtils.toString(quat_diff_scenes,use_degrees));
		        for (int ns = 0; ns < quat_scenes.length; ns++) {
		        	quat_scenes[ns] = QuatUtils.sceneRelLocalGround(
		        			alt_datas[ns], // double []   txy,
		        			remove_rs? null:affines[ns], // double [][] affine,
		        			y_down_ccw); // boolean y_down_ccw)
			        System.out.println("quat_scenes["+ns+"]=    "+QuatUtils.toString(quat_scenes[ns],use_degrees));
			        // 0, 1 and simulated 2?  
		        }
		        double [] quat1a = QuatUtils.multiplyScaled(quat_scenes[0], quat_diff_scenes);
		        double [] quat1b = QuatUtils.multiplyScaled(quat_diff_scenes, quat_scenes[0]);
		        System.out.println("quat1a =           "+QuatUtils.toString(quat1a,use_degrees));
		        System.out.println("quat1b =           "+QuatUtils.toString(quat1b,use_degrees)); // reversed order
		        double aff_qscenes [][][] = new double [quat_scenes.length][][];
		        SingularValueDecomposition[] svd_acenes = new SingularValueDecomposition[ aff_qscenes.length];
		        for (int ns = 0; ns < aff_qscenes.length; ns++) {
		        	aff_qscenes[ns] = QuatUtils.quatToAffine(quat_scenes[ns] ,invert_q2a,true);
		        	svd_acenes[ns]=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff_qscenes[ns], y_down_ccw);
		        	System.out.println(QuatUtils.affinesToString(aff_qscenes[ns], "aff_qscenes["+ns+"]"));
			        System.out.println("svd_acenes["+ns+"]=" + svd_acenes[ns].toString(use_degrees));
			        System.out.println();
		        	
		        }
		        System.out.println("================== compare diff affines w/o rot and scale");
		        SingularValueDecomposition svd_affine_pair_nors =
		        		SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_pair_nors, y_down_ccw);
	        	System.out.println(QuatUtils.affinesToString(affine_pair_nors, "affine_pair_nors"));
	        	System.out.println("svd_affine_pair_nors= " + svd_affine_pair_nors.toString(use_degrees));
		        System.out.println();
	        	double [][] affine_tilt_diff = QuatUtils.diffAffineFromTilts(
	        			alt_datas[0], // double [] tilts0,
	        			alt_data,     // double [] tilts_diff,
	        			invert_q2a, // boolean invert_q2a) // false
	        			debug_tilts,
	        			invert_order, // boolean invert_order,
	        			invert_y); // boolean invert_y)
		        SingularValueDecomposition svd_affine_tilt_diff =
		        		SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_tilt_diff, y_down_ccw);
	        	System.out.println(QuatUtils.affinesToString(affine_tilt_diff, "affine_tilt_diff"));
	        	System.out.println("svd_affine_tilt_diff= " + svd_affine_tilt_diff.toString(use_degrees));
		        System.out.println();

		        double [][] adq_err =  QuatUtils.matMult2x2(affine_tilt_diff, QuatUtils.matInverse2x2(affine_pair_nors));
		        SingularValueDecomposition svd_adq_err=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(adq_err, y_down_ccw);
	        	System.out.println(QuatUtils.affinesToString(adq_err, "adq_err"));
		        System.out.println("svd_adq_err=   " + svd_adq_err.toString(use_degrees));
		        System.out.println();
		        
		        System.out.println("+++++++++++++ Relative affine from quat2 to quat0 (from tilts, should match affine_pair)");
		        
	        	System.out.println(QuatUtils.affinesToString(affine_pair, "affine_pair  "));
	        	System.out.println("svd_affine_pair= " + svd_affine_pair.toString(use_degrees));

		        double [][] adq20 =  QuatUtils.matMult2x2(aff_qscenes[2], QuatUtils.matInverse2x2(aff_qscenes[0]));
		        SingularValueDecomposition svd_adq20=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(adq20, y_down_ccw);
	        	System.out.println(QuatUtils.affinesToString(adq20, "adq20   "));
		        System.out.println("svd_adq20=   " + svd_adq20.toString(use_degrees));
		        System.out.println();

		        double [][] adq20_err =  QuatUtils.matMult2x2(adq20, QuatUtils.matInverse2x2(affine_pair));
		        SingularValueDecomposition svd_adq20_err=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(adq20_err, y_down_ccw);
	        	System.out.println(QuatUtils.affinesToString(adq20_err, "adq20_err"));
		        System.out.println("svd_adq20_err=    " + svd_adq20_err.toString(use_degrees));
		        System.out.println();

//		        double [][] adq20i =  QuatUtils.matMult2x2(QuatUtils.matInverse2x2(aff_qscenes[0]),aff_qscenes[2]);
		        double [][] adq20i =  QuatUtils.matInverse2x2(adq20);
		        SingularValueDecomposition svd_adq20i=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(adq20i, y_down_ccw);
	        	System.out.println(QuatUtils.affinesToString(adq20i, "adq20i  "));
		        System.out.println("svd_adq20i=  " + svd_adq20i.toString(use_degrees));
		        System.out.println();
		        
		        
		        /*
		        double [][] iadq20 = QuatUtils.matInverse2x2(adq20);
		        SingularValueDecomposition svd_iadq20=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(iadq20, y_down_ccw);
	        	System.out.println(QuatUtils.affinesToString(iadq20, "iadq20   "));
		        System.out.println("svd_iadq20=   " + svd_iadq20.toString(use_degrees));
		        System.out.println();
		        */
		        
		        double [][] affq1a =QuatUtils.quatToAffine(quat1a ,invert_q2a,true);
		        SingularValueDecomposition svd_affq1a=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affq1a, y_down_ccw);
	        	System.out.println(QuatUtils.affinesToString(affq1a, "affq1a   "));
		        System.out.println("svd_affq1a=   " + svd_affq1a.toString(use_degrees));
		        System.out.println();

		        
		        double [][] affq1b =QuatUtils.quatToAffine(quat1b ,invert_q2a,true);
		        SingularValueDecomposition svd_affq1b=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affq1b, y_down_ccw);
	        	System.out.println(QuatUtils.affinesToString(affq1b, "affq1b   "));
		        System.out.println("svd_affq1b=   " + svd_affq1b.toString(use_degrees));
		        System.out.println();
		        
		        // combine aff_qscenes[0] and affine_pair
		        double [][] aff_gnd2 = QuatUtils.matMult2x2(affine_pair, aff_qscenes[0]);
		        SingularValueDecomposition saff_gnd2=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff_gnd2, y_down_ccw);
	        	System.out.println(QuatUtils.affinesToString(aff_gnd2, "aff_gnd2  "));
		        System.out.println("saff_gnd2=   " + saff_gnd2.toString(use_degrees));
		        System.out.println();
		        // convert aff_gnd2 to a pair of quaternions
	        	double[][] qaff2_gnd_pm = QuatUtils.affineToQuatScaled(aff_gnd2, false, y_down_ccw);
		        double [][] qaff2_gnd_diff = {
		        		QuatUtils.divideScaled(qaff2_gnd_pm[0], quat_scenes[2]),
		        		QuatUtils.divideScaled(qaff2_gnd_pm[1], quat_scenes[2])
		        };
	        	
	        	
		        System.out.println("Should match:");
		        System.out.println("quat_scenes[2] =    "+QuatUtils.toString(quat_scenes[2],use_degrees)); // reversed order
		        System.out.println("qaff2_gnd_pm[0] =   "+QuatUtils.toString(qaff2_gnd_pm[0],use_degrees)); // reversed order
		        System.out.println("qaff2_gnd_diff[0] = "+QuatUtils.toString(qaff2_gnd_diff[0],use_degrees)); // reversed order
		        System.out.println("qaff2_gnd_pm[1] =   "+QuatUtils.toString(qaff2_gnd_pm[1],use_degrees)); // reversed order
		        System.out.println("qaff2_gnd_diff[1] = "+QuatUtils.toString(qaff2_gnd_diff[1],use_degrees)); // reversed order
		        System.out.println();
		        
	            double [][] qvariants = {
	            		QuatUtils.divideScaled(qaff2_gnd_pm[0], quat_scenes[0]),
	            		QuatUtils.divideScaled(qaff2_gnd_pm[1], quat_scenes[0])};
		        double [][] qvariants_diff = {
		        		QuatUtils.divideScaled(qvariants[0], quat_diff_scenes),
		        		QuatUtils.divideScaled(qvariants[1], quat_diff_scenes)
		        };
		        System.out.println("qvariants[0] = "+QuatUtils.toString(qvariants[0],use_degrees)); // reversed order
		        System.out.println("qvar_diff[0] = "+QuatUtils.toString(qvariants_diff[0],use_degrees)); // reversed order
		        System.out.println("qvariants[1] = "+QuatUtils.toString(qvariants[1],use_degrees)); // reversed order
		        System.out.println("qvar_diff[1] = "+QuatUtils.toString(qvariants_diff[1],use_degrees)); // reversed order
		        System.out.println();
		        
		        // test 
	        	double[][] qaff1_gnd_pm = QuatUtils.affineToQuatScaled(aff_qscenes[0], false, y_down_ccw);
	            double [][] qvariants_test = {
	            		QuatUtils.divideScaled(qaff1_gnd_pm[0], quat_scenes[0]),
	            		QuatUtils.divideScaled(qaff1_gnd_pm[1], quat_scenes[0])};
//		        double [][] qvariants_test_diff = {
//		        		QuatUtils.divideScaled(qvariants_test[0], quat_diff_scenes),
//		        		QuatUtils.divideScaled(qvariants_test[0], quat_diff_scenes)
//		        };
		        System.out.println("tvariants[0] = "+QuatUtils.toString(qvariants_test[0],use_degrees)); // reversed order
//		        System.out.println("tvar_diff[0] = "+QuatUtils.toString(qvariants_test_diff[0],use_degrees)); // reversed order
		        System.out.println("tvariants[1] = "+QuatUtils.toString(qvariants_test[1],use_degrees)); // reversed order
//		        System.out.println("tvar_diff[1] = "+QuatUtils.toString(qvariants_test_diff[1],use_degrees)); // reversed order
		        System.out.println();

		        
			}

	        if (test_quat) {
	        	System.out.println(">>>>>>>>>>>>>>>>> npair="+npair+": "+ipair[0]+" -> "+ipair[1]);
	        	
	        	double [] quat_diff = QuatUtils.tiltToQuaternion(
	        			alt_data,
	        			y_down_ccw); // boolean y_down_ccw)
	        	double [][] quats01 = new double [alt_datas.length][];
		        alt_datas[2] = new double [3];
		        for (int i = 0; i < alt_datas[2].length; i++) {
		        	alt_datas[2][i] = alt_datas[0][i]+alt_data[i];
		        }
		        for (int ns = 0; ns < quats01.length; ns++) {
		    		quats01[ns] = QuatUtils.tiltToQuaternion(
		    				alt_datas[ns],
		        			y_down_ccw); // boolean y_down_ccw)
		        }
		        double [] quat_rdiff = QuatUtils.invert(quat_diff);
		        
		        double [] quat2a =   QuatUtils.multiply(quats01[0], quat_diff);
		        double [] quat2b =   QuatUtils.multiply(quat_diff,  quats01[0]);
		        double [] quat2ap =  QuatUtils.pureTilt(quat2a); 
		        double [] quat2bp =  QuatUtils.pureTilt(quat2b);
		        //pureTilt
		        System.out.println("alt_data=    ["+alt_data[0]+","+alt_data[1]+","+alt_data[2]+"]");
		        System.out.println("alt_datas[0]=["+alt_datas[0][0]+","+alt_datas[0][1]+","+alt_datas[0][2]+"]");
		        System.out.println("alt_datas[1]=["+alt_datas[1][0]+","+alt_datas[1][1]+","+alt_datas[1][2]+"]");
		        System.out.println("alt_datas[2]=["+alt_datas[2][0]+","+alt_datas[2][1]+","+alt_datas[2][2]+"]");

		        System.out.println("quat2a=      ["+quat2a[0]+","+quat2a[1]+","+quat2a[2]+","+quat2a[3]+"] "+QuatUtils.norm(quat2a));
		        System.out.println("quat2b=      ["+quat2b[0]+","+quat2b[1]+","+quat2b[2]+","+quat2b[3]+"] "+QuatUtils.norm(quat2b));
		        System.out.println("quat2ap=     ["+quat2ap[0]+","+quat2ap[1]+","+quat2ap[2]+","+quat2ap[3]+"] "+QuatUtils.norm(quat2ap));
		        System.out.println("quat2bp=     ["+quat2bp[0]+","+quat2bp[1]+","+quat2bp[2]+","+quat2bp[3]+"] "+QuatUtils.norm(quat2bp));
		        double [][] affine_pair = pairwiseOrthoMatch.getAffine();
		        
		        // calculate second from first and pair
		        double [][] affine0 = ortho_maps[ipair[0]].getAffine();
				Matrix A0 = new Matrix (
						new double [][] {{affine0[0][0],affine0[0][1]},{affine0[1][0],affine0[1][1]}});
				Matrix A10 = new Matrix (
						new double [][] {{affine_pair[0][0],affine_pair[0][1]},{affine_pair[1][0],affine_pair[1][1]}});
		        Matrix A1a = A10.times(A0);
		        double [][] affine1a = new double[][] {
					{A1a.get(0,0),A1a.get(0,1)},
					{A1a.get(1,0),A1a.get(1,1)}};
		        double [][][] affines  = new double [][][] {ortho_maps[ipair[0]].getAffine(),ortho_maps[ipair[1]].getAffine(), affine1a};
		        boolean   make__pure_tilt = false;

		        double [][] aff1_stretch = QuatUtils.quatToAffine( // use old for stretch
		        		quats01[0],       // double [] quat,
		    			true,             //   boolean   stretch,
		    			make__pure_tilt, // boolean   make__pure_tilt)
		    			y_down_ccw); // boolean y_down_ccw);
		        double [][] aff1_shrink = QuatUtils.quatToAffine(
		        		quats01[0],       // double [] quat,
		        		invert_q2a,            //   boolean   stretch,
		    			y_down_ccw); // boolean y_down_ccw);
		        double [][] aff2_shrink = QuatUtils.quatToAffine(
		        		quats01[2],       // double [] quat,
		        		invert_q2a,             //   boolean   stretch,
		    			y_down_ccw); // boolean y_down_ccw);
		        double [][] aff2_stretch = QuatUtils.quatToAffine( // use old for stretch
		        		quats01[2],       // double [] quat,
		    			true,             //   boolean   stretch,
		    			make__pure_tilt, // boolean   make__pure_tilt)
		    			y_down_ccw); // boolean y_down_ccw);
		        
		        double [][] qaffd_pm =    QuatUtils.affineToQuatScaled(affine_pair,true,  y_down_ccw);
		        System.out.println("qaffd_pm[0]=    "+QuatUtils.toString(qaffd_pm[0],use_degrees));
		        System.out.println("qaffd_pm[1]=    "+QuatUtils.toString(qaffd_pm[1],use_degrees));
		        
		        double [][] qaff1a_pm =    QuatUtils.affineToQuatScaled(aff1_shrink, false,y_down_ccw);
		        System.out.println("qaff1a_pm[0]=   "+QuatUtils.toString(qaff1a_pm[0],use_degrees));
		        System.out.println("qaff1a_pm[1]=   "+QuatUtils.toString(qaff1a_pm[1],use_degrees));
		        
		        double [][] qaff1_pm =    QuatUtils.affineToQuatScaled(aff1_stretch, true, y_down_ccw);
		        System.out.println("qaff1_pm[0]=    "+QuatUtils.toString(qaff1_pm[0],use_degrees));
		        System.out.println("qaff1_pm[1]=    "+QuatUtils.toString(qaff1_pm[1],use_degrees));
		        
		        double [][] qaff2a_pm =   QuatUtils.affineToQuatScaled(aff2_shrink, false, y_down_ccw);
		        System.out.println("qaff2a_pm[0]=   "+QuatUtils.toString(qaff2a_pm[0],use_degrees));
		        System.out.println("qaff2a_pm[1]=   "+QuatUtils.toString(qaff2a_pm[1],use_degrees));
		        
		        double [][] qaff2_pm =    QuatUtils.affineToQuatScaled(aff2_stretch, true, y_down_ccw);
		        System.out.println("qaff2_pm[0]=    "+QuatUtils.toString(qaff2_pm[0],use_degrees));
		        System.out.println("qaff2_pm[1]=    "+QuatUtils.toString(qaff2_pm[1],use_degrees));
		        
		        double [][] qaffine0_pm = QuatUtils.affineToQuatScaled(affines[0], false, y_down_ccw);
		        double [][] qa0_pm = {qaffine0_pm[0].clone(),qaffine0_pm[1].clone()}; 
		        System.out.println("qaffine0_pm[0]= "+QuatUtils.toString(qaffine0_pm[0],use_degrees));
		        System.out.println("qaffine0_pm[1]= "+QuatUtils.toString(qaffine0_pm[1],use_degrees));
		        
		        double [][] qaffine1_pm = QuatUtils.affineToQuatScaled(affines[1], false, y_down_ccw);
		        double [][] qa1_pm = {qaffine1_pm[0].clone(),qaffine1_pm[1].clone()}; 
		        System.out.println("qaffine1_pm[0]= "+QuatUtils.toString(qaffine1_pm[0],use_degrees));
		        System.out.println("qaffine1_pm[1]= "+QuatUtils.toString(qaffine1_pm[1],use_degrees));

		        double [][] qaffine1a_pm = QuatUtils.affineToQuatScaled(affines[2], false, y_down_ccw);
		        double [][] qa1a_pm = {qaffine1a_pm[0].clone(),qaffine1a_pm[1].clone()}; 
		        System.out.println("qaffine1a_pm[0]="+QuatUtils.toString(qaffine1a_pm[0],use_degrees));
		        System.out.println("qaffine1a_pm[1]="+QuatUtils.toString(qaffine1a_pm[1],use_degrees));
		        
		        double [][][][] raffines_pm = new double [affines.length][2][][];
		        SingularValueDecomposition[][] rsvd_pm =       new SingularValueDecomposition [affines.length][2];
		        for (int i = 0; i < raffines_pm.length;i++) {
		        	raffines_pm[i][0] = QuatUtils.quatToAffine(QuatUtils.affineToQuatScaled(affines[i], false, y_down_ccw)[0],invert_q2a,true);
		        	raffines_pm[i][1] = QuatUtils.quatToAffine(QuatUtils.affineToQuatScaled(affines[i], false, y_down_ccw)[1],invert_q2a,true);
		        	rsvd_pm[i][0] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(raffines_pm[i][0], y_down_ccw);
		        	rsvd_pm[i][1] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(raffines_pm[i][1], y_down_ccw);
			        System.out.println("raffines["+i+"][0]= [["+raffines_pm[i][0][0][0]+ ","+raffines_pm[i][0][0][1]+"],");
			        System.out.println(    "                 ["+raffines_pm[i][0][1][0]+ ","+raffines_pm[i][0][1][1]+"]]");
			        System.out.println("raffines["+i+"][1]= [["+raffines_pm[i][1][0][0]+ ","+raffines_pm[i][1][0][1]+"],");
			        System.out.println(    "                 ["+raffines_pm[i][1][1][0]+ ","+raffines_pm[i][1][1][1]+"]]");
			        System.out.println("rsvd_pm["+i+"][0]= "+ rsvd_pm[i][0].toString(use_degrees));
			        System.out.println("rsvd_pm["+i+"][1]= "+ rsvd_pm[i][1].toString(use_degrees));

			        System.out.println();
		        }
		        
		        
/*
		        double [][] qvariants = {
		        		QuatUtils.divideScaled(qa0_pm[0], qa1_pm[0]),
		        		QuatUtils.divideScaled(qa0_pm[1], qa1_pm[0]),
		        		QuatUtils.divideScaled(qa0_pm[0], qa1_pm[1]),
		        		QuatUtils.divideScaled(qa0_pm[1], qa1_pm[1])};
		        double [][] qvariants1 = {
		        		QuatUtils.divideScaled1(qa0_pm[0], qa1_pm[0]),
		        		QuatUtils.divideScaled1(qa0_pm[1], qa1_pm[0]),
		        		QuatUtils.divideScaled1(qa0_pm[0], qa1_pm[1]),
		        		QuatUtils.divideScaled1(qa0_pm[1], qa1_pm[1])};
		        double [] svariants=  new double [qvariants.length];
		        double [] svariants1= new double [qvariants1.length];
		        for (int i = 0; i < svariants.length; i++)	svariants[i] = QuatUtils.normalizeInPlace(qvariants[i]); 
		        for (int i = 0; i < svariants1.length; i++)	svariants1[i] = QuatUtils.normalizeInPlace(qvariants1[i]); 
		        
		        for (int i = 0; i < svariants.length; i++)	{
		        	System.out.println(" qvariant["+i+"] = ["+qvariants[i][0]+ ","+qvariants[i][1]+ ","+qvariants[i][2]+ ","+qvariants[i][3]+ "] scale="+svariants[i]);
		        	System.out.println("qvariant1["+i+"] = ["+qvariants1[i][0]+ ","+qvariants1[i][1]+ ","+qvariants1[i][2]+ ","+qvariants1[i][3]+ "] scale="+svariants1[i]);
		        }

 */
		        
		        
		        double [][] qvariants = {
		        		QuatUtils.divideScaled(qa1_pm[0], qa0_pm[0]),
		        		QuatUtils.divideScaled(qa1_pm[1], qa0_pm[0]),
		        		QuatUtils.divideScaled(qa1_pm[0], qa0_pm[1]),
		        		QuatUtils.divideScaled(qa1_pm[1], qa0_pm[1])};
		        /*
		        double [][] qvariants1 = {
		        		QuatUtils.divideScaled1(qa1_pm[0], qa0_pm[0]),
		        		QuatUtils.divideScaled1(qa1_pm[1], qa0_pm[0]),
		        		QuatUtils.divideScaled1(qa1_pm[0], qa0_pm[1]),
		        		QuatUtils.divideScaled1(qa1_pm[1], qa0_pm[1])};
		        		*/
		        double [] svariants=  new double [qvariants.length];
		        double [][][] taffines = new double [qvariants.length][][];
		        SingularValueDecomposition[] svd_avars = new SingularValueDecomposition[qvariants.length];
		        for (int i = 0; i < svariants.length; i++)	svariants[i] = QuatUtils.normalizeInPlace(qvariants[i]); 
		        
		        for (int i = 0; i < svariants.length; i++)	{
		        	System.out.println(" qvariant["+i+"] = ["+qvariants[i][0]+ ","+qvariants[i][1]+ ","+qvariants[i][2]+ ","+qvariants[i][3]+ "] scale="+svariants[i]);
		        	taffines[i] = QuatUtils.quatToAffine(
		        			qvariants[i],       // double [] quat,
		        			invert_q2a,         //   boolean   invert,
			    			y_down_ccw); // boolean y_down_ccw);
		        	svd_avars[i] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(taffines[i], y_down_ccw);
		        }
		        
		        double [][] qvariantsa = {
		        		QuatUtils.divideScaled(qa1a_pm[0], qa0_pm[0]),
		        		QuatUtils.divideScaled(qa1a_pm[1], qa0_pm[0]),
		        		QuatUtils.divideScaled(qa1a_pm[0], qa0_pm[1]),
		        		QuatUtils.divideScaled(qa1a_pm[1], qa0_pm[1])};
		        double [][] qvariantsa_diff = new double [qvariantsa.length][2];
		        for (int i = 0; i < qvariantsa_diff.length; i++)	{
		        	qvariantsa_diff[i] = QuatUtils.divideScaled(qvariantsa[i], quat_diff);
		        }
		        
		        System.out.println("\n--------------------------------");
		        System.out.println("quat_diff=     "+QuatUtils.toString(quat_diff,use_degrees));
		        System.out.println();
		        for (int i = 0; i < qvariantsa.length; i++)	{
		        	System.out.println("qvarianta["+i+"] = "+QuatUtils.toString(qvariantsa[i],use_degrees));
		        }
		        System.out.println();
		        for (int i = 0; i < qvariantsa_diff.length; i++)	{
		        	System.out.println("qvar_diff["+i+"] = "+QuatUtils.toString(qvariantsa_diff[i],use_degrees));
		        }
		        System.out.println("^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^");
		        
		        
		        for (int i = 0; i < qvariantsa.length; i++)	{
		        	System.out.println("qvarianta["+i+"] = "+QuatUtils.toString(qvariantsa[i],use_degrees));
			        System.out.println("taffines["+i+"]=    [["+taffines[i][0][0]+ ","+taffines[i][0][1]+"]");
			        System.out.println(    "                 ["+taffines[i][1][0]+ ","+taffines[i][1][1]+"]]");
			        System.out.println("svd_avars["+i+"]= "+svd_avars[i].toString(use_degrees));
			        System.out.println();
		        }
		        
		        
		        double [][] aff_combo = QuatUtils.matMult(aff2_shrink,aff1_stretch); // invert order?
		        double [][] aff_combo1= QuatUtils.matMult(aff1_stretch,aff2_shrink); // invert order?
		        SingularValueDecomposition svd_affine_pair =  SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_pair, y_down_ccw); // boolean y_down_ccw)
		        SingularValueDecomposition[] svd_affines =    {
		        		SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[0], y_down_ccw),
		        		SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[1], y_down_ccw),
		        		SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[2], y_down_ccw)};
		        SingularValueDecomposition svd_aff1_shrink =  SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff1_shrink, y_down_ccw);
		        SingularValueDecomposition svd_aff1_stretch = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff1_stretch, y_down_ccw);
		        SingularValueDecomposition svd_aff2_shrink =  SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff2_shrink, y_down_ccw);
		        SingularValueDecomposition svd_aff2_stretch = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff2_stretch, y_down_ccw);
		        SingularValueDecomposition svd_aff_combo =    SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff_combo, y_down_ccw);
		        SingularValueDecomposition svd_aff_combo1 =   SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff_combo1, y_down_ccw);
		        
		        System.out.println("affine_pair=    [["+affine_pair[0][0]+","+affine_pair[0][1]+","+affine_pair[0][2]+"]");
		        System.out.println("                 ["+affine_pair[1][0]+","+affine_pair[1][1]+","+affine_pair[1][2]+"]]");
		        System.out.println("svd_affine_pair= " + svd_affine_pair.toString(use_degrees));
		        System.out.println();
	        
		        System.out.println("affines[0]=     [["+affines[0][0][0]+ ","+affines[0][0][1]+ ","+affines[0][0][2]+"]");
		        System.out.println("                 ["+affines[0][1][0]+ ","+affines[0][1][1]+ ","+affines[0][1][2]+"]]");
		        System.out.println("svd_affines[0]=  " + svd_affines[0].toString(use_degrees));
		        System.out.println();
		        
		        System.out.println("affines[1]=     [["+affines[1][0][0]+ ","+affines[1][0][1]+ ","+affines[1][0][2]+"]");
		        System.out.println("                 ["+affines[1][1][0]+ ","+affines[1][1][1]+ ","+affines[1][1][2]+"]]");
		        System.out.println("svd_affines[1]=  " + svd_affines[1].toString(use_degrees));
		        System.out.println();
		        
		        System.out.println("affines[2]=     [["+affines[2][0][0]+ ","+affines[2][0][1]+"]");
		        System.out.println("                 ["+affines[2][1][0]+ ","+affines[2][1][1]+"]]");
		        System.out.println("svd_affines[2]=  " + svd_affines[2].toString(use_degrees));
		        System.out.println();
		        
		        System.out.println("aff1_shrink=   [["+aff1_shrink[0][0]+ ","+aff1_shrink[0][1]+"]");
		        System.out.println("                 ["+aff1_shrink[1][0]+ ","+aff1_shrink[1][1]+"]]");
		        System.out.println("svd_aff1_shrink=" + svd_aff1_shrink.toString(use_degrees));
		        System.out.println();

		        System.out.println("aff1_stretch=   [["+aff1_stretch[0][0]+ ","+aff1_stretch[0][1]+"]");
		        System.out.println("                 ["+aff1_stretch[1][0]+ ","+aff1_stretch[1][1]+"]]");
		        System.out.println("svd_aff1_stretch=" + svd_aff1_stretch.toString(use_degrees));
		        System.out.println();

		        System.out.println("aff2_shrink=    [["+aff2_shrink[0][0]+  ","+aff2_shrink[0][1]+ "]");
		        System.out.println("                 ["+aff2_shrink[1][0]+  ","+aff2_shrink[1][1]+ "]]");
		        System.out.println("svd_aff2_shrink= " + svd_aff2_shrink.toString(use_degrees));
		        System.out.println();
		        
		        System.out.println("aff2_stretch=   [["+aff2_stretch[0][0]+ ","+aff2_stretch[0][1]+"]");
		        System.out.println("                 ["+aff2_stretch[1][0]+ ","+aff2_stretch[1][1]+"]]");
		        System.out.println("svd_aff2_stretch=" + svd_aff2_stretch.toString(use_degrees));
		        System.out.println();
		        
		        System.out.println("aff_combo=      [["+aff_combo[0][0]+  ","+aff_combo[0][1]+ "]");
		        System.out.println("                 ["+aff_combo[1][0]+  ","+aff_combo[1][1]+ "]]");
		        System.out.println("svd_aff_combo=   " + svd_aff_combo.toString(use_degrees));
		        System.out.println();
		        
		        System.out.println("aff_combo1=     [["+aff_combo1[0][0]+ ","+aff_combo1[0][1]+ "]");
		        System.out.println("                 ["+aff_combo1[1][0]+ ","+aff_combo1[1][1]+ "]]");
		        System.out.println("svd_aff_combo1=  " + svd_aff_combo1.toString(use_degrees));
		        System.out.println();
//		        System.out.println("scale,tilt_rad,gamma,rot");
		        System.out.println("quat_diff=   "+QuatUtils.toString(quat_diff,use_degrees));
		        System.out.println("quats01[0]=  "+QuatUtils.toString(quats01[0],use_degrees));
		        System.out.println("quats01[1]=  "+QuatUtils.toString(quats01[1],use_degrees));
		        System.out.println("quats01[2]=  "+QuatUtils.toString(quats01[2],use_degrees));
		        //QuatUtils.toString(qvariantsa[i],use_degrees));
		        System.out.println();
		        System.out.println();
		        System.out.println();
	        }
    		
    		
	        
//	        double [] alt_data = new double[3];
//	        System.arraycopy(alt_data5, 0, alt_data, 0, alt_data.length);
	        if (!do_not_save) {
	        	pairwiseOrthoMatch.setAltData(alt_data);
	        }
	        
			if (log_append && (log_path != null)) { // assuming directory exists
				StringBuffer sb = new StringBuffer();
				sb.append(String.format("%4d\t%4d\t%s\t%s\t%6.4f\t%3d\t%6.4f\t%7.4f\t%7.4f\t%7.3f\n",
						ipair[0], ipair[1], ortho_maps[ipair[0]].getName(), ortho_maps[ipair[1]].getName(),
						pairwiseOrthoMatch.overlap, pairwiseOrthoMatch.zoom_lev, pairwiseOrthoMatch.rms,
						pairwiseOrthoMatch.alt_data[0],pairwiseOrthoMatch.alt_data[1],pairwiseOrthoMatch.alt_data[2]));
				CalibrationFileManagement.saveStringToFile (
						log_path,          //String path,
						sb.toString(), // data,
						true); // boolean append)
			}
	        
	        
	        
	 //pix_size_meters   	
	    	
	    	/*
        	double agl_ratio = max_agl/50.0;
        	double metric_error_adj = metric_error * agl_ratio * agl_ratio; // metric_error settings is good for 50m. Increase for higher Maybe squared?
	    	int initial_zoom = max_zoom_lev - 4;  // another algorithm?
	    	// overlaps
	    	double overlap_frac = pairwiseOrthoMatch.getOverlap(); // .pairsGraph.getOverlap(next_pair);
			// unityAffine()
	    	// use unityAffine() for 0; getaffine for second?
			double [][] affine0 = OrthoMapsCollection.unityAffine(); // ortho_maps[ipair[0]].getAffine();
			double [][] affine1 = daffine; // ortho_maps[ipair[1]].getAffine();
			double [][][] affines = new double[][][] {affine0,affine1};
			boolean success = true;
			Point pair = new Point(ipair[0],ipair[1]);
			double lores_rms = pairwiseOrthoMatch.getRMS();
			// high-res

			affines[1][0] = affines[1][0].clone(); 
			affines[1][1] = affines[1][1].clone();
			Rectangle woi = new Rectangle(); // used to return actual woi from correlateOrthoPair()		
			correlateOrthoPair(
					clt_parameters,  // CLTParameters    clt_parameters,
					pairwiseOrthoMatch, //PairwiseOrthoMatch pairwiseOrthoMatch, // will return statistics
					0,                // 			int              min_overlap,
					max_std,         // double           max_std,      // maximal standard deviation to limit center area  
					min_std_rad,     // double           min_std_rad,  // minimal radius of the central area (if less - fail)
					frac_remove,     // double           frac_remove, //  =        0.25
					metric_error_adj,// 			double           metric_error,
					ignore_prev_rms, // boolean ignore_prev_rms,
					num_tries,       //  = 5int              num_tries, //  = 5
					false, // ,            // boolean          calc_warp, (will return null if false)
					batch_mode,      // boolean          batch_mode,
					ipair,           // String []        gpu_spair,
					affines,         // double [][][]    affines, // on top of GPS offsets
					woi,             // Rectangle        woi,
					min_zoom_lev,    // int              zoom_lev,
					false,           // show_vf,         // boolean show_vf,
					null,            // ground_planes,   // double [][]      ground_planes, // null or double[2] - will return ground planes
					rad_fraction,    // double           rad_fraction,
					max_tile_rad,    // double           max_tile_rad, //  = 30;
					fill_fraction,   // double           fill_fraction,
					fill_fraction_final, // double           fill_fraction_final,
					ease_nosfm,      // double           ease_nosfm,
					null,            // double []        max_rms_iter, //  = {1.0, 0.6};//
					pull_skew,       // double           pull_skew,        // ~rotation, = 0 fraction of the total weight == 1
					pull_tilt,       // double           pull_tilt,        // > 0
					pull_scale,      // double           pull_scale,       // = 0
					debugLevel-4);   // final int        debugLevel)
	*/		
		}
//		double tilt_err_threshold = 0.01; // reduce weight if larger
//overlop_pow = 2.0		
        /*
		for (int npair = 0; npair <quat_pairs.length; npair++ ) {
			System.out.println("quat_pairs["+npair+"]="+QuatUtils.toString(quat_pairs[npair],use_degrees));
		}
		double [] quat02 = QuatUtils.multiplyScaled(quat_pairs[0],quat_pairs[2]);
		System.out.println(    "quat02=       "+QuatUtils.toString(quat02,use_degrees));
		double [] quat_err = QuatUtils.multiplyScaled(quat02,  QuatUtils.invertScaled(quat_pairs[1]));
		System.out.println(    "quat_err=     "+QuatUtils.toString(quat_err,use_degrees));

		double quat02_norm =   QuatUtils.norm(quat02);
		double quat02_offs =   Math.sqrt(quat02[1]*quat02[1]+quat02[2]*quat02[2]+quat02[3]*quat02[3]);
		double quat02_orient = quat02_offs/quat02_norm;
		double quat02_scale =  quat02_norm-1.0;

		double quat_err_norm = QuatUtils.norm(quat_err);
		double quat_err_offs = Math.sqrt(quat_err[1]*quat_err[1]+quat_err[2]*quat_err[2]+quat_err[3]*quat_err[3]);
		double orient_err = quat_err_offs/quat_err_norm;
		double scale_err = quat_err_norm-1.0;
		
		System.out.println("quat02_orient="+quat02_orient);
		System.out.println("quat02_scale= "+quat02_scale);

		System.out.println("orient_err="+orient_err);
		System.out.println("scale_err= "+scale_err);

		System.out.println("orient_err_rel="+orient_err/quat02_orient);
		System.out.println("scale_err_rel= "+scale_err/quat02_scale);

		
		for (int npair = 0; npair <quat_pairs.length; npair++ ) {
		    SingularValueDecomposition svd =  SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_pairs[npair], y_down_ccw); // boolean y_down_ccw)
		    System.out.println("svd_affine["+npair+"]=" + svd.toString(use_degrees));
		}
		*/
		
		double max_flat_err = 0; // tilt_err_threshold
		for (int npair = 0; npair < num_pairs; npair++) {
			max_flat_err = Math.max(max_flat_err, flat_err[npair]);
			//weights_pairs
			weights_pairs[npair] = tilt_err_threshold/Math.max(tilt_err_threshold, flat_err[npair])*Math.pow(overlaps[npair], overlop_pow);
		}
		System.out.println("max_flat_err="+max_flat_err);
		
		OrientationSceneLMA orientationSceneLMA = new OrientationSceneLMA();
		orientationSceneLMA.prepareLMA(
				indices,         // int    []     indices, // should all be used
				condensed_pairs, // int    [][]   cpairs,
				weights_pairs,   // double []     weights_pairs, // from matching tilts(flatness) (and worst sfm, per-pair rmse)?
				weight_rot,      // double        weight_rot,    // >0 weight of pairs errors in qn3
				weight_tilt,     // double        weight_tilt,   // >0 weight of pairs errors in qn1, qn2
				weight_scale,    // double        weight_scale,  // >0 weight in pairs scale-1.0 errors
				pull,            // double        pull,          // 0 <= pull <1 - fraction of all RMS contributors
				pull_rots,       // double        pull_rots,     // >=0 weight of sum of rotations, may be 0, normalized by pull value 
				pull_tilts,      // double        pull_tilts,    // >=0 weights of sum of qn1 and qn2 of scenes, normalized by pull value
				pull_scales,     // double        pull_scales,   // >=0 weights of scales of scenes, normalized by pull value
				quat_pairs,      // double [][]   qpairs,        // [pair][4] quaternions for pairs - orientation and scale (non-unity quaternions) 
				debugLevel);     // int           debug_level);
		double  lambda =            0.1;
		double  lambda_scale_good = 0.5;
		double  lambda_scale_bad =  8.0;
		double  lambda_max =      1000;
		boolean last_run =       false;
		double  rms_diff =          0.0001;
		int     num_iter = 100;
		int lma_rslt= orientationSceneLMA.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
				lambda, // double lambda,           // 0.1
				lambda_scale_good,// double lambda_scale_good,// 0.5
				lambda_scale_bad, // double lambda_scale_bad, // 8.0
				lambda_max,       // double lambda_max,       // 100
				rms_diff,         // double rms_diff,         // 0.001
				num_iter,         //int    num_iter,         // 20
				last_run,         // boolean last_run,
				null, // String dbg_prefix,
				debugLevel);      // int    debug_level)
		if (lma_rslt >= 0) {
			System.out.println("LMA -> "+lma_rslt);
			double [][] qorients = orientationSceneLMA.getOrientationQuaternions();
			for (int nscene = 0; nscene < indices.length; nscene++) {
				ortho_maps[indices[nscene]].setQOrient(qorients[nscene]);
				
			}
			for (int npair = 0; npair < quat_pairs.length; npair++) {
				int [] cpair = condensed_pairs[npair]; // index alt_multi
				int [] ipair = {indices[cpair[0]], indices[cpair[1]]};
				PairwiseOrthoMatch pairwiseOrthoMatch = ortho_maps[ipair[0]].getMatch(ortho_maps[ipair[1]].getName(), true); // ?
				pairwiseOrthoMatch.setQuaternion(quat_pairs[npair]);
			}
		}
		
		
		/*
		
		ERSTiltLMA ersTiltLMA = new  ERSTiltLMA();
		ersTiltLMA.prepareLMA(
				indices,           // int    []     indices, // should all be used
				condensed_pairs,   // int    [][]   cpairs,
				weights_scenes,    // double []     weights_scenes, // sfm, number used?
				weights_pairs,     // double []     weights_pairs, // from matching tilts(flatness) (and worst sfm, per-pair rmse)?
				weight_pairs_k,    // double        weight_pairs_k,
				scene_tilts_pairs, // double [][][] tilts,      // [pair][scene(2)][tilt(2)]
				affine_pairs,      // double [][][] affine_pairs,
				debugLevel);       // int           debug_level)
		lma_rslt=ersTiltLMA.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
				lambda, // double lambda,           // 0.1
				lambda_scale_good,// double lambda_scale_good,// 0.5
				lambda_scale_bad, // double lambda_scale_bad, // 8.0
				lambda_max,       // double lambda_max,       // 100
				rms_diff,         // double rms_diff,         // 0.001
				num_iter,         //int    num_iter,         // 20
				last_run,         // boolean last_run,
				null, // String dbg_prefix,
				debugLevel);      // int    debug_level)
		System.out.println("LMA -> "+lma_rslt);
		if (lma_rslt >= 0) {
			if (debugLevel > -3) {
				ersTiltLMA.printSceneResults(use_degrees, use_percents);
				
				ersTiltLMA.printPairsResults(use_degrees, use_percents);				
			}
		}
		System.out.println();
		if (debugLevel > 2) {
			for (int npair = 0; npair < ersTiltLMA.num_pairs; npair++) {
				double [][]   pseudo_xy= ersTiltLMA.getPseudoXY(npair);
				QuatUtils.testPseudoAffineDiffAndDerivatives(
						pseudo_xy, // double [][]   xy,   // [scene][direction]
						ersTiltLMA.aff_tilts[npair], // double [][][] a_tilts,
						ersTiltLMA.aff_pairs_inosr[npair], // double [][]   iaff_pair, // affine pair inversed
						invert_q2a); // boolean       invert_q2a){  // invert result affines (to match "usual")
				System.out.println();
			}

		}
		
		*/
		
		//printResults(boolean degrees)
		if (orthoMapsCollection_path != null) {
        	try {
        		orthoMapsCollection.writeOrthoMapsCollection(orthoMapsCollection_path);
        	} catch (IOException e) {
        		// TODO Auto-generated catch block
        		e.printStackTrace();
        	}
        	if (debugLevel > -4) {
        		System.out.println("Saved data to "+ orthoMapsCollection_path);
        	}
		}
		return true;
    }
	
	
	
}
