/**
 ** 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.Point;
import java.awt.Rectangle;
import java.io.IOException;
import java.text.SimpleDateFormat;
import java.util.ArrayList;
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,
			// log/save parameters
			boolean             save_each,
			boolean             log_append,
			String              log_path,
			String              orthoMapsCollection_path,
			int                 debugLevel) {
		boolean y_down_ccw = true;
		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];
		
//		int num_pairs = 0; // available_pairs.length
//		ArrayList<Point> failed_pairs = new ArrayList<Point>();
		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]);
			}
			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++) {
    			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) {
    	        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]};
    		
    		boolean test_quat = true;
	        if (test_quat) {
	        	System.out.println(">>>>>>>>>>>>>>>>> npair="+npair+": "+ipair[0]+" -> "+ipair[1]);
		        boolean use_degrees = true;
	        	
		        boolean [][] masks = new boolean[2][];
		        double [][] alt_data5s = new double[2][];
	        	double [][] data_overlap = new double[2][];
	        	double [][] alt_datas = new double[3][];
	        	double [] quat_diff = QuatUtils.tiltToQuaternion(
	        			alt_data,
	        			y_down_ccw); // boolean y_down_ccw)
	        	double [][] quats01 = new double [alt_datas.length][];
		        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++) {
		    			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) {
		    	        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]};
		        }
		        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 [] 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);
		        /*
		        double [] quat2an =  QuatUtils.normalize(quat2a); 
		        double [] quat2bn =  QuatUtils.normalize(quat2b); 
		        double [] quat2apn = QuatUtils.normalize(quat2ap); 
		        double [] quat2bpn = QuatUtils.normalize(quat2bp);
		        */ 

		        /*
		        double [] quat1ar = QuatUtils.multiply(quats01[1], quat_rdiff);
		        double [] quat1br = QuatUtils.multiply(quat_rdiff,  quats01[1]);
		        double [] quat1cr = QuatUtils.multiply(quats01[2], quat_rdiff);
		        double [] quat1dr = QuatUtils.multiply(quat_rdiff,  quats01[2]);
		        */
		        /*
		        double [] quat1apr = QuatUtils.pureTilt(quat1ar); 
		        double [] quat1bpr = QuatUtils.pureTilt(quat1br); 
		        double [] quat1cpr = QuatUtils.pureTilt(quat1cr); 
		        double [] quat1dpr = QuatUtils.pureTilt(quat1dr);
		        */ 
		        /*
		        double [] quat1aprn = QuatUtils.normalize(quat1apr); 
		        double [] quat1bprn = QuatUtils.normalize(quat1bpr); 
		        double [] quat1cprn = QuatUtils.normalize(quat1cpr); 
		        double [] quat1dprn = QuatUtils.normalize(quat1dpr); 
		        double [] quat1arn = QuatUtils.normalize(quat1ar); 
		        double [] quat1brn = QuatUtils.normalize(quat1br); 
		        double [] quat1crn = QuatUtils.normalize(quat1cr); 
		        double [] quat1drn = QuatUtils.normalize(quat1dr);
		        */ 
		        //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("quat_diff=   ["+quat_diff[0]+","+quat_diff[1]+","+quat_diff[2]+","+quat_diff[3]+"] "+QuatUtils.norm(quat_diff));
		        System.out.println("quats01[0]=  ["+quats01[0][0]+","+quats01[0][1]+","+quats01[0][2]+","+quats01[0][3]+"] "+QuatUtils.norm(quats01[0]));
		        System.out.println("quats01[1]=  ["+quats01[1][0]+","+quats01[1][1]+","+quats01[1][2]+","+quats01[1][3]+"] "+QuatUtils.norm(quats01[1]));
		        System.out.println("quats01[2]=  ["+quats01[2][0]+","+quats01[2][1]+","+quats01[2][2]+","+quats01[2][3]+"] "+QuatUtils.norm(quats01[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));
/*
		        System.out.println("quat2an=     ["+quat2an[0]+","+quat2an[1]+","+quat2an[2]+","+quat2an[3]+"] "+QuatUtils.norm(quat2an));
		        System.out.println("quat2bn=     ["+quat2bn[0]+","+quat2bn[1]+","+quat2bn[2]+","+quat2bn[3]+"] "+QuatUtils.norm(quat2bn));
		        System.out.println("quat2apn=    ["+quat2apn[0]+","+quat2apn[1]+","+quat2apn[2]+","+quat2apn[3]+"] "+QuatUtils.norm(quat2apn));
		        System.out.println("quat2bpn=    ["+quat2bpn[0]+","+quat2bpn[1]+","+quat2bpn[2]+","+quat2bpn[3]+"] "+QuatUtils.norm(quat2bpn));
		        System.out.println("quat1apr=    ["+quat1apr[0]+","+quat1apr[1]+","+quat1apr[2]+","+quat1apr[3]+"] "+QuatUtils.norm(quat1apr)); // +
		        System.out.println("quat1bpr=    ["+quat1bpr[0]+","+quat1bpr[1]+","+quat1bpr[2]+","+quat1bpr[3]+"] "+QuatUtils.norm(quat1bpr)); // ++
		        System.out.println("quat1cpr=    ["+quat1cpr[0]+","+quat1cpr[1]+","+quat1cpr[2]+","+quat1cpr[3]+"] "+QuatUtils.norm(quat1cpr)); // +++
		        System.out.println("quat1dpr=    ["+quat1dpr[0]+","+quat1dpr[1]+","+quat1dpr[2]+","+quat1dpr[3]+"] "+QuatUtils.norm(quat1dpr)); // +++
		        System.out.println("quat1aprn=   ["+quat1aprn[0]+","+quat1aprn[1]+","+quat1aprn[2]+","+quat1aprn[3]+"] "+QuatUtils.norm(quat1aprn)); // +
		        System.out.println("quat1bprn=   ["+quat1bprn[0]+","+quat1bprn[1]+","+quat1bprn[2]+","+quat1bprn[3]+"] "+QuatUtils.norm(quat1bprn)); // ++
		        System.out.println("quat1cprn=   ["+quat1cprn[0]+","+quat1cprn[1]+","+quat1cprn[2]+","+quat1cprn[3]+"] "+QuatUtils.norm(quat1cprn)); // +++
		        System.out.println("quat1dprn=   ["+quat1dprn[0]+","+quat1dprn[1]+","+quat1dprn[2]+","+quat1dprn[3]+"] "+QuatUtils.norm(quat1dprn)); // +++
		        System.out.println("quat1arn=    ["+quat1arn[0]+","+quat1arn[1]+","+quat1arn[2]+","+quat1arn[3]+"] "+QuatUtils.norm(quat1arn)); // +
		        System.out.println("quat1brn=    ["+quat1brn[0]+","+quat1brn[1]+","+quat1brn[2]+","+quat1brn[3]+"] "+QuatUtils.norm(quat1brn)); // ++
		        System.out.println("quat1crn=    ["+quat1crn[0]+","+quat1crn[1]+","+quat1crn[2]+","+quat1crn[3]+"] "+QuatUtils.norm(quat1crn)); // +++
		        System.out.println("quat1drn=    ["+quat1drn[0]+","+quat1drn[1]+","+quat1drn[2]+","+quat1drn[3]+"] "+QuatUtils.norm(quat1drn)); // +++
		        */
		        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(
		        		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,
		    			false,            //   boolean   stretch,
		    			make__pure_tilt, // boolean   make__pure_tilt)
		    			y_down_ccw); // boolean y_down_ccw);
		        double [][] aff2_shrink = QuatUtils.quatToAffine(
		        		quats01[2],       // double [] quat,
		    			false,             //   boolean   stretch,
		    			make__pure_tilt, // boolean   make__pure_tilt)
		    			y_down_ccw); // boolean y_down_ccw);
		        double [][] aff2_stretch = QuatUtils.quatToAffine(
		        		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],false,true,true);
		        	raffines_pm[i][1] = QuatUtils.quatToAffine(QuatUtils.affineToQuatScaled(affines[i], false, y_down_ccw)[1],false,false,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];
///		        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]);
		        	taffines[i] = QuatUtils.quatToAffine(
		        			qvariants[i],       // double [] quat,
			    			false,             //   boolean   stretch,
			    			false, // make__pure_tilt, // boolean   make__pure_tilt)
			    			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])};
		        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); // invdert order?
		        double [][] aff_combo1= QuatUtils.matMult(aff1_stretch,aff2_shrink); // invdert 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);
	        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)
	*/		
		}
		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;
    }
	
	
	
}
