/**
 ** OrthoMap - Dealing with orthographic maps 
 **
 ** Copyright (C) 2024 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  QuatUtils.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 com.elphel.imagej.common.GenericJTabbedDialog;

public final class QuatUtils { // static class
	public static final double []   UNIT_QUAT =   {1,0,0,0};
	public static final double [][] UNIT_AFFINE = {{1,0},{0,1}};
	private QuatUtils() {}
	/*
	 * t= r*s
	 * (t0, t1, t2, t3) = (r0, r1, r2, r3) * (s0, s1, s2, s3) 
	 * t0 = (r0s0 - r1s1 - r2s2 - r3s3)
     * t1 = (r0s1 + r1s0 - r2s3 + r3s2)
     * t2 = (r0s2 + r1s3 + r2s0 - r3s1)
     * t3 = (r0s3 - r1s2 + r2s1 + r3s0)
	 */

	public static String toString(
			double [] quat_i,
			boolean degrees) {
		double [] quat=quat_i.clone();
		double scale =normalizeInPlace(quat);
		String fmt_rad = "[%12.9f, %12.9f,%12.9f, %12.9f], tilt=%12.9f, dir=%12.9f, scale = %12.10f";
		String fmt_deg = "[%12.9f, %12.9f,%12.9f, %12.9f], tilt=%12.7f\u00B0, dir=%12.7f\u00B0, scale=%12.10f";
		double s = degrees ? (180/Math.PI):1;
		String fmt=degrees ? fmt_deg : fmt_rad;
		String rslt=String.format(fmt, quat[0], quat[1],quat[2],quat[3], s*2*Math.acos(quat[0]), s*Math.atan2(quat[2],quat[1]), scale);
		return rslt;
	}
	
	/**
	 * Multiply to quaternions
	 * @param r first quaternion
	 * @param s second quaternion
	 * @return product of two quaternions: r*s
	 */
	public static double [] multiply(
			double [] r,
			double [] s	) {
		double [] t = {
				 r[0]*s[0] - r[1]*s[1] - r[2]*s[2] - r[3]*s[3],
			     r[0]*s[1] + r[1]*s[0] - r[2]*s[3] + r[3]*s[2],
			     r[0]*s[2] + r[1]*s[3] + r[2]*s[0] - r[3]*s[1],
			     r[0]*s[3] - r[1]*s[2] + r[2]*s[1] + r[3]*s[0]};
		return t;
	}
	/**
	 * Differential d_(p*q)/dp
	 * https://faculty.sites.iastate.edu/jia/files/inline-files/quaternion.pdf
	 * Seems that (11) for []x is transposed!
	 * @param p quaternion
	 * @param q quaternion
	 * @return matrix[4][4], where rows correspond to elements of quaternion (p*q), and columns - indices of p,
	 * so this matrix when multiplied by a column vector dp results in column vector d(pq) 
	 */
	public static double [][] d_pq_dp(
			double [] p,
			double [] q) {
		return new double[][] {
			{q[0],-q[1],-q[2], -q[3]},
			{q[1], q[0],-q[3],  q[2]},
			{q[2], q[3], q[0], -q[1]},
			{q[3],-q[2], q[1],  q[0]}  
/*
			{q[0],-q[1],-q[2], -q[3]},
			{q[1], q[0], q[3], -q[2]},
			{q[2],-q[3], q[0],  q[1]},
			{q[3], q[2],-q[1],  q[0]}  

 */
		
		};
	}

	public static double [][] d_pq_dp(
			double [] q) {
		return new double[][] {
			{q[0],-q[1],-q[2], -q[3]},
			{q[1], q[0],-q[3],  q[2]},
			{q[2], q[3], q[0], -q[1]},
			{q[3],-q[2], q[1],  q[0]}  
		};
	}
	
	
	
	public static double [][] d_pq_dp(
			double [] p,
			double [] q,
			double delta) {
		double [][] d_pq_dp_delta=new double[4][4];
		for (int npar = 0; npar < 4; npar++) {
			double [] vpm = p.clone();
			vpm[npar] += 0.5*delta;
			double [] qd_p = multiply(vpm, q);
			vpm[npar] -= delta;
			double [] qd_m = multiply(vpm, q);
			for (int i = 0; i < 4; i++) {
				d_pq_dp_delta[i][npar] = (qd_p[i]-qd_m[i])/delta;
			}
		}
		return d_pq_dp_delta;
	}
	
	
	
	/**
	 * Differential d_(p*q)/dq
	 * https://faculty.sites.iastate.edu/jia/files/inline-files/quaternion.pdf
	 * Seems that (11) for []x is transposed!
	 * @param p quaternion
	 * @param q quaternion
	 * @return matrix[4][4], where rows correspond to elements of quaternion (p*q), and columns - indices of q,
	 * so this matrix when multiplied by a column vector dq results in column vector d(pq) 
	 */
	public static double [][] d_pq_dq(
			double [] p,
			double [] q) {
		return new double[][] {
			{p[0],-p[1],-p[2], -p[3]},
			{p[1], p[0], p[3], -p[2]},
			{p[2],-p[3], p[0],  p[1]},
			{p[3], p[2],-p[1],  p[0]}
/*
			{p[0],-p[1],-p[2], -p[3]},
			{p[1], p[0],-p[3],  p[2]},
			{p[2], p[3], p[0], -p[1]},
			{p[3],-p[2], p[1],  p[0]}  

 */
		};
	}

	public static double [][] d_pq_dq(
			double [] p) {
		return new double[][] {
			{p[0],-p[1],-p[2], -p[3]},
			{p[1], p[0], p[3], -p[2]},
			{p[2],-p[3], p[0],  p[1]},
			{p[3], p[2],-p[1],  p[0]}		};
	}

	
	
	public static double [][] d_pq_dq(
			double [] p,
			double [] q,
			double delta) {
		double [][] d_pq_dq_delta=new double[4][4];
		for (int npar = 0; npar < 4; npar++) {
			double [] vpm = q.clone();
			vpm[npar] += 0.5*delta;
			double [] qd_p = multiply(p, vpm);
			vpm[npar] -= delta;
			double [] qd_m = multiply(p, vpm);
			for (int i = 0; i < 4; i++) {
				d_pq_dq_delta[i][npar] = (qd_p[i]-qd_m[i])/delta;
			}
		}
		return d_pq_dq_delta;
	}
	
	
	
	/**
	 * Derivative of inverse quanternion
	 * @param q quternion
	 * @return derivative as 4x4 array
	 */
	public static double [][] d_invert_dq (
			double [] q){
		double q0= q[0],q1=q[1],q2=q[2],q3=q[3];
		double l2 =q0*q0+q1*q1+q2*q2+q3*q3;
		double l2l2=l2*l2;
		return new double[][] {
			{ (l2 - 2*q0*q0)/l2l2,       -2*q0*q1/l2l2,       -2*q0*q2/l2l2,       -2*q0*q3/l2l2}, 
			{       2*q1*q0/ l2l2, (2*q1*q1 - l2)/l2l2,        2*q1*q2/l2l2,        2*q1*q3/l2l2}, 
			{       2*q2*q0/ l2l2,        2*q2*q1/l2l2, (2*q2*q2 - l2)/l2l2,        2*q2*q3/l2l2}, 
			{       2*q3*q0/ l2l2,        2*q3*q1/l2l2,        2*q3*q2/l2l2, (2*q3*q3 - l2)/l2l2}, 
		};
	}
	
	public static double [][] d_invert_dq(
			double [] q,
			double delta) {
		double [][] d_invert_dq_delta=new double[4][4];
		for (int npar = 0; npar < 4; npar++) {
			double [] vpm = q.clone();
			vpm[npar] +=  0.5*delta;
			double [] qd_p = invert(vpm);
			vpm[npar] -= delta;
			double [] qd_m = invert(vpm);
			for (int i = 0; i < 4; i++) {
				d_invert_dq_delta[i][npar] = (qd_p[i]-qd_m[i])/delta;
			}
		}
		return d_invert_dq_delta;
	}

	
	
	public static double [][] dnormalize_dq(
			double [] q){
		double q0= q[0],q1=q[1],q2=q[2],q3=q[3];
		double l2 =q0*q0+q1*q1+q2*q2+q3*q3;
		double l3=l2*Math.sqrt(l2);
		return new double[][] {
			{(l2 - q0*q0)/l3,      -q0*q1 /l3,      -q0*q2 /l3,      -q0*q3 /l3},
			{     -q1*q0 /l3, (l2 - q1*q1)/l3,      -q1*q2 /l3,      -q1*q3 /l3},
			{     -q2*q0 /l3,      -q2*q1 /l3, (l2 - q2*q2)/l3,      -q2*q3 /l3},
			{     -q3*q0 /l3,      -q3*q1 /l3,      -q3*q2 /l3, (l2 - q3*q3)/l3}
		};		
	}
	
	public static double [][] dnormalize_dq(
			double [] q,
			double delta) {
		double [][] dnormalize_dq_delta=new double[4][4];
		for (int npar = 0; npar < 4; npar++) {
			double [] vpm = q.clone();
			vpm[npar] +=  0.5*delta;
			double [] qd_p = normalize(vpm);
			vpm[npar] -= delta;
			double [] qd_m = normalize(vpm);
			for (int i = 0; i < 4; i++) {
				dnormalize_dq_delta[i][npar] = (qd_p[i]-qd_m[i])/delta;
			}
		}
		return dnormalize_dq_delta;
	}

	
	
	public static double [] dscale_dq(
			double [] q){
		double q0= q[0],q1=q[1],q2=q[2],q3=q[3];
		double l = Math.sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
		return new double [] {q0/l, q1/l, q2/l,q3/l}; 
	}
	
	public static double [] dscale_dq(
			double [] q,
			double delta) {
		double [] dscale_dq_delta=new double[4];
		for (int npar = 0; npar < 4; npar++) {
			double [] vpm = q.clone();
			vpm[npar] +=  0.5*delta;
			double  qd_p = norm(vpm);
			vpm[npar] -= delta;
			double  qd_m = norm(vpm);
			dscale_dq_delta[npar] = (qd_p-qd_m)/delta;
		}
		return dscale_dq_delta;
	}
	
	
	
	/**
	 * Differential d_(p*q'))/dp (q' - q-conjugated)
	 * https://faculty.sites.iastate.edu/jia/files/inline-files/quaternion.pdf
	 * @param p quaternion
	 * @param q quaternion
	 * @return matrix[4][4], where rows correspond to elements of quaternion (p*q), and columns - indices of p,
	 * so this matrix when multiplied by a column vector dp results in column vector d(pq') 
	 */
	/*
	public static double [][] d_pqc_dp(
			double [] p,
			double [] q) {
		return new double[][] {
			{ q[0], q[1], q[2],  q[3]},
			{-q[1], q[0],-q[3],  q[2]},
			{-q[2], q[3], q[0], -q[1]},
			{-q[3],-q[2], q[1],  q[0]}  
		};
	}
	*/
	
	/**
	 * Differential d_(p*q')/dq    (q' - q-conjugated)
	 * https://faculty.sites.iastate.edu/jia/files/inline-files/quaternion.pdf
	 * @param p quaternion
	 * @param q quaternion
	 * @return matrix[4][4], where rows correspond to elements of quaternion (p*q), and columns - indices of q,
	 * so this matrix when multiplied by a column vector dq results in column vector d(pq) 
	 */
	/*
	public static double [][] d_pqc_dq(
			double [] p,
			double [] q) {
		return new double[][] {
			{p[0], p[1], p[2],  p[3]},
			{p[1],-p[0], p[3], -p[2]},
			{p[2],-p[3],-p[0],  p[1]},
			{p[3], p[2],-p[1], -p[0]}  
		};
	}
	*/
	
	
	public static double [] multiplyScaled( // Not needed, same result as multiply()
			double [] r,
			double [] s	) {
		return scale(multiply(normalize(r),normalize(s)),norm(r)*norm(s));
	}
	
	public static double [] invertScaled(double [] quat) {
		return scale(invert(normalize(quat)),1.0/norm(quat));
	}
	
	
	
	public static double [] divide( // pure rotation
			double [] r,
			double [] s	) {
		return multiply(r, invert(s));
	}	

	public static double [] divideScaled(
			double [] r,
			double [] s	) {
		return multiplyScaled(r, invertScaled(s));
	}	

	public static double [] divideScaled1(
			double [] r,
			double [] s	) {
		return multiply(r, invertScaled(s));
	}	
	
	public static void testQuatAff() {
		double rot1_deg = 5.0;
		double dir1_deg = 80.0;
		double tilt1_deg = 10.0;
		double scale1 = 1.0;
		double rot2_deg = 15.0;
		double dir2_deg = 20.0;
		double tilt2_deg = 12.0;
		double scale2 = 1.1;
		boolean run = true;
		boolean invert_q2a= true;
		boolean y_down_ccw = true;
		System.out.println("rot1_deg="+rot1_deg+", dir1_deg="+dir1_deg+", tilt1_deg="+tilt1_deg+", scale1="+scale1);
		System.out.println("rot2_deg="+rot2_deg+", dir2_deg="+dir2_deg+", tilt2_deg="+tilt2_deg+", scale2="+scale2);
		System.out.println("invert_q2a="+invert_q2a+", y_down_ccw="+y_down_ccw);
		
		while (run) {
			double rot1 =  rot1_deg*Math.PI/180;
			double dir1 =  dir1_deg*Math.PI/180;
			double tilt1 = tilt1_deg*Math.PI/180;
			double rot2 =  rot2_deg*Math.PI/180;
			double dir2 =  dir2_deg*Math.PI/180;
			double tilt2 = tilt2_deg*Math.PI/180;
			System.out.println("-----------------------------------");
			System.out.println("rot1_deg="+rot1_deg+", dir1_deg="+dir1_deg+", tilt1_deg="+tilt1_deg+", scale1="+scale1);
			System.out.println("rot2_deg="+rot2_deg+", dir2_deg="+dir2_deg+", tilt2_deg="+tilt2_deg+", scale2="+scale2);
			System.out.println("rot1="+rot1+", dir1="+dir1+", tilt1="+tilt1+", scale1="+scale1);
			System.out.println("rot2="+rot2+", dir2="+dir2+", tilt2="+tilt2+", scale2="+scale2);
			System.out.println("invert_q2a="+invert_q2a+", y_down_ccw="+y_down_ccw);
			double [] quat1 = quatRotDirTiltScale(rot1, dir1,tilt1,scale1);
			double [] quat2 = quatRotDirTiltScale(rot2, dir2,tilt2,scale2);
//	        System.out.println("quats01[2]=  "+toString(quats01[2],use_degrees));
			testQuatAff(quat1, quat2, invert_q2a, y_down_ccw);
			System.out.println("run="+run);
		}
	}
	public static void testQuatAff(
			double [] quat1,
			double [] quat2,
			boolean invert_q2a,
			boolean y_down_ccw) {
		boolean use_degrees= true;
		double [] quat_diff = divideScaled(quat2, quat1);
        System.out.println("quat1=         "+toString(quat1,use_degrees));
        System.out.println("quat2=         "+toString(quat2,use_degrees));
        System.out.println("quat_diff=     "+toString(quat_diff,use_degrees));
        double [][] quats= {quat1,quat2,quat_diff};
        double [][][] affines = new double [quats.length][][];
        SingularValueDecomposition [] svds= new SingularValueDecomposition [quats.length];
        /*
        for (int i = 0; i < quats.length; i++){
        	affines[i] = quatToAffine(quats[i],	    invert_q2a,y_down_ccw);
        	String name = (i==2)? "_diff" : "["+i+"]  ";
        	System.out.println(affinesToString(affines[i], "affines"+name));
        	svds[i] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[i], y_down_ccw);
	        System.out.println("svd_affines"+name+" = "+svds[i].toString(use_degrees));
        }
        */
        for (int i = 0; i < 2; i++){
        	affines[i] = quatToAffine(quats[i],	    invert_q2a,y_down_ccw);
        	System.out.println(affinesToString(affines[i], "affines["+i+"]  "));
        	svds[i] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[i], y_down_ccw);
	        System.out.println("svd_affines["+i+"] =   "+svds[i].toString(use_degrees));
        }
        // get differential affines
        affines[2] = matMult2x2 (affines[1], matInverse2x2(affines[0]));
    	System.out.println(affinesToString(affines[2], "affines_diff"));
    	svds[2] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[2], y_down_ccw);
        System.out.println("svd_affines_diff = "+svds[2].toString(use_degrees));
        
        double [][][] quats_pm = new double [2][][];
        double [][][] quats_diff_pm = new double [2][2][];
        for (int i = 0; i <2; i++) {
        	quats_pm[i] = affineToQuatScaled(affines[i], false, y_down_ccw);
        	quats_diff_pm[i][0]= divideScaled(quats_pm[i][0], quats[i]);
        	quats_diff_pm[i][1]= divideScaled(quats_pm[i][1], quats[i]);
        }
        
        System.out.println("quat1=         "+toString(quat1,use_degrees));
        System.out.println("quats_pm[0][0]="+toString(quats_pm[0][0],use_degrees));
        System.out.println("quats_pm[0][1]="+toString(quats_pm[0][1],use_degrees));
        System.out.println("qdiff_pm[0][0]="+toString(quats_diff_pm[0][0],use_degrees));
        System.out.println("qdiff_pm[0][1]="+toString(quats_diff_pm[0][1],use_degrees));
        System.out.println();
        System.out.println("quat2=         "+toString(quat2,use_degrees));
        System.out.println("quats_pm[1][0]="+toString(quats_pm[1][0],use_degrees));
        System.out.println("quats_pm[1][1]="+toString(quats_pm[1][1],use_degrees));
        System.out.println("qdiff_pm[1][0]="+toString(quats_diff_pm[1][0],use_degrees));
        System.out.println("qdiff_pm[1][1]="+toString(quats_diff_pm[1][1],use_degrees));
        System.out.println();
        double [][] qvariants = {
        		divideScaled(quats_pm[1][0], quats_pm[0][0]),
        		divideScaled(quats_pm[1][1], quats_pm[0][0]),
        		divideScaled(quats_pm[1][0], quats_pm[0][1]),
        		divideScaled(quats_pm[1][1], quats_pm[0][1])};
        System.out.println("quat_diff=     "+toString(quat_diff,use_degrees));
        double [][] qvariants_diff = new double [qvariants.length][2];
        for (int i = 0; i < qvariants_diff.length; i++)	{
        	qvariants_diff[i] = divideScaled(qvariants[i], quat_diff);
        }
        
        for (int i = 0; i < qvariants.length; i++)	{
            System.out.println("quat_var["+i+"]=   "+toString(qvariants[i],use_degrees));
        }
        System.out.println();
        for (int i = 0; i < qvariants.length; i++)	{
            System.out.println("qvar_diffs["+i+"]= "+toString(qvariants_diff[i],use_degrees));
        }
        return;
	}
	
	public static String affinesToString(
			double [][] a,
			String      name) {
		boolean max_is_scale = false;
		boolean show_diff_unity=true;
		int flen = name.length();
		String fmt1 = String.format("%%%ds = [[",flen);
		String fmt2 = String.format("%%%ds    [",flen);
		String s = "";
		for (int i = 0; i < a.length; i++) {
			if (i==0) s+= String.format(fmt1, name);
			else      s+= String.format(fmt2, "");
			for (int j=0; j < a[i].length; j++) {
				s += String.format("%12.9f", a[i][j]);
				if (j < (a[i].length-1)) {
					s+= ", ";
				} else {
					if (i < (a.length-1)) {
						s += "],\n";
					} else {
						s += "]]";
					}
				}
			}
		}
		if (show_diff_unity) {
			double [][] a_noS = SingularValueDecomposition.removeTiltRotScale(
					a,             // double [][] A,
					false,         // boolean removeTilt,
					false,         // boolean removeRot,
					true,          // boolean removeScale,
					false,         // boolean removeOffset,
					max_is_scale); // boolean max_is_scale);
			double [][] a_noRS = SingularValueDecomposition.removeTiltRotScale(
					a,             // double [][] A,
					false,         // boolean removeTilt,
					true,          // boolean removeRot,
					true,          // boolean removeScale,
					false,         // boolean removeOffset,
					max_is_scale); // boolean max_is_scale);

			double mag =      diffUnity(a);

			double mag_noS =  diffUnity(a_noS);
			double mag_noRS = diffUnity(a_noRS);
			s+=String.format(", magnitude=%12.9f, mag_no_scale=%12.9f mag_tilts=%12.9f", mag, mag_noS, mag_noRS);
		}
		
		
		return s;
	}
	
	public static double diffUnity(
			double [][] a) {
		return Math.sqrt((a[0][0]-1)*(a[0][0]-1) + (a[1][1]-1)*(a[1][1]-1) + a[0][1]*a[0][1] + a[1][0]*a[1][0]);
	}
	
	
	
	public static double [] quatRotDirTiltScale(
			double rot,
			double dir,
			double tilt,
			double scale) {
		double chalfrot = Math.cos(rot/2); 
		double shalfrot = Math.sin(rot/2); 
		double [] qrot = {chalfrot,0,0,shalfrot}; // rotation around vertical axis
		// rotation after rot is -beta (rot(beta)*W*rot(-beta)*rot(rot)
		double beta = dir;;  
		double cmbeta = Math.cos(-beta);
		double smbeta = Math.sin(-beta);
		double tiltAngle = tilt; // >0
		double ctilt = Math.cos(tiltAngle/2);
		double stilt = Math.sin(tiltAngle/2);
		double [] q_plus =  {ctilt,  stilt*cmbeta,  stilt*smbeta, 0};
//		double [] q_minus = {ctilt, -stilt*cmbeta, -stilt*smbeta, 0};
		double [] qs = scale(multiply(qrot,q_plus), scale);
		return qs;

	}
	
	
	
	
	public static double norm(
			double [] quat) {
		return Math.sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]);
	}
	public static double [] invert0(
			double [] quat) {
		return new double [] {quat[0], -quat[1], -quat[2], -quat[3]};
	}

	public static double [] invert( // make sure did not break anything
			double [] quat) {
		double s2 = quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3];
		return scale(new double [] {quat[0], -quat[1], -quat[2], -quat[3]},1/s2);
	}
	
	
	
	public static double [] normalize(
			double [] quat) {
//		double k = 1/norm(quat);
//		return new double [] {k*quat[0], k*quat[1], k*quat[2], k*quat[3]};
		return scale (quat, 1/norm(quat));
	}
	public static double normalizeInPlace(
			double [] quat) {
		double scale = norm(quat);
		for (int i = 0; i < quat.length; i++) {
			quat[i] /= scale;
		}
		return scale;
	}
	
	public static double [] scale(
			double [] quat,
			double k) {
		return new double [] {k*quat[0], k*quat[1], k*quat[2], k*quat[3]};
	}	
	/**
	 * Convert two tilts {tx,ty} Z=x*tx+y*ty to quaternion. Source y is down, quaternion - x - same, y - opposite (positive - up), z - up from the XY plane. 
	 * @param txy {tiltX, tiltY,...} - may contain other values like offset and center
	 * @param y_down_ccw Invert Y for tilts
	 * @return plane rotation quaternion rotation righth-hand thread
	 */
	public static double [] tiltToQuaternion(
			double [] txy,
			boolean	y_down_ccw) { // boolean y_down_ccw)
		double tx = txy[0], ty = y_down_ccw?-txy[1]:txy[1]; // invert y
		double t2 = tx*tx + ty*ty;
		double t = Math.sqrt(t2);
		if (t == 0) {
			return UNIT_QUAT;
		}
		tx/=t;
		ty/=t;
		
		double [] axis = {ty,-tx,0}; // pi/2 CW
		
		/* sin (A/2) = +-sqrt((1 - cos(A))/2)
		 * cos (A/2) = +-sqrt((1 + cos(A))/2)
		 * cos(A) = sqrt(t*t+1)
		 */
		double cos_theta = 1/Math.sqrt(t2+1);
		double cos_theta2 = Math.sqrt((1 + cos_theta)/2);
		double sin_theta2 = Math.sqrt((1 - cos_theta)/2);
		double [] quat = new double[] {cos_theta2, sin_theta2*axis[0], sin_theta2*axis[1], 0};
		return quat;
	}
	
	public static String tiltToString (
			double [] txy,
			boolean	y_down_ccw,
			boolean degrees) {
		String fmt_rad = " tilt= %10.7f, dir=%10.7f";
		String fmt_deg = " tilt= %10.5f\u00B0, dir=%10.5f\u00B0";
		double k = degrees ? (180/Math.PI):1;
		String fmt=degrees ? fmt_deg : fmt_rad;

		String s = String.format(" = [%12.9f, %12.9f, %15.9f]", txy[0], txy[1], txy[2]);
		double tx = txy[0], ty = y_down_ccw?-txy[1]:txy[1]; // invert y
		double t2 = tx*tx + ty*ty;
		double t = Math.sqrt(t2);
		if (t == 0) {
			return s;
		}
		tx/=t;
		ty/=t;
		
		double [] axis = {ty,-tx,0}; // pi/2 CW
		double dir = Math.atan2(axis[1],axis[0]);
		double tilt = Math.atan(t);
		
		s+= String.format(fmt, k*tilt, k*dir);
		return s;
	}
	
	
	public static double [] sceneRelLocalGround(
			double []   txy,
			double [][] affine, // used for rot and scale Now can be null
			boolean	    y_down_ccw) { // boolean y_down_ccw) for tilts and affine
		double [] qtilts = tiltToQuaternion(txy,y_down_ccw);
		boolean inv_aff = true; // invert affine
		double scale = 1.0;
		double rot = 0;
		if (affine != null) {
			if (inv_aff) {
				affine = matInverse2x2(affine);
			}
			SingularValueDecomposition svd= SingularValueDecomposition.singularValueDecomposeScaleTiltBeta(
					affine,
					y_down_ccw);
			scale = svd.getMaxScale();
			rot = svd.getRotAngle(); // TODO: check sign !
		}		
		
		// Now beta,rot correspond to Y - up
		double chalfrot = Math.cos(rot/2); 
		double shalfrot = Math.sin(rot/2); 
		double [] qrot = {chalfrot,0,0,shalfrot}; // rotation around vertical axis
		
		double [] qinv_tilts = invert(qtilts); //scene to ground
//		double [] qrt=  multiply(qrot,qinv_tilts); // just for debugging
		double [] quat= scale(multiply(qinv_tilts,qrot),scale);
		return    quat;
	}

	
	/**
	 * No scale/rotation
	 * @param txy
	 * @param y_down_ccw
	 * @return
	 */
	public static double [] sceneRelLocalGround(
			double []   txy,
			boolean	    y_down_ccw) { // boolean y_down_ccw) for tilts and affine
		double [] qtilts = tiltToQuaternion(txy,y_down_ccw);
		double [] qinv_tilts = invert(qtilts); //scene to ground
		return    qinv_tilts;
	}
	
	
	
	/**
	 * Calculate relative affine transform from scene0 to scene1
	 * from tilts of the first scene and differential tilt from 
	 * scene0 to scene1 (tilt1 - tilt0). Individual scene tilt
	 * measurement depends on finding ground planes that may be 
	 * inacurate due to non-flat surfaces, so differential tilt
	 * is much better. Tilts are calculated after scene rotations
	 * are corrected, so the result affine does not include rotation
	 * and scale (they should be removed from the differential
	 * affine when comparing, This method is intended for fitting
	 * tilts0 (first manually, then with LMA) to get the best fit
	 * between the differential tilt and differential affine
	 * transform and so resolving ambiguity of the sign of affine
	 * tilt. 
	 * @param tilts0 first scene tilts (tiltX, tiltY, optional offset)
	 * @param tilts_diff differential tilt (scene1 - scene0)
	 * @param invert_q2a invert affines from quternions to match "usual" ones
	 * @return differential affine transform (no scale and rotation)
	 */
	public static double[][] diffAffineFromTilts(
			double [] tilts0,
			double [] tilts_diff,
			boolean invert_q2a,  // invert result affines (to match "usual")
			boolean debug){
		return diffAffineFromTilts(
				tilts0,
				tilts_diff,
				invert_q2a,  // invert result affines (to match "usual")
				debug,
				false, // boolean invert_order,
				false); // boolean invert_y)
	}
	public static double[][] diffAffineFromTilts(
			double [] tilts0,
			double [] tilts_diff,
			boolean invert_q2a,  // invert result affines (to match "usual")
			boolean debug,
			boolean invert_order,
			boolean invert_y) {
		boolean	 y_down_ccw = true;
		
		double [] tilts1 = tilts0.clone();
		for (int i = 0; i < tilts_diff.length;i++) {
			tilts1[i] += tilts_diff[i];
		}
		double [][] tilts = {tilts0, tilts1};
		double [][][] affines= new double [2][][];
		double [] aff_tilts = new double[2];
		for (int nscene = 0; nscene < 2; nscene++) {
			/*
			double [] quat = sceneRelLocalGround(
	    			tilts[nscene], // double []   txy,
	    			null, // double [][] affine,
	    			y_down_ccw); // boolean y_down_ccw)
			affines[nscene] = quatToAffine(quat ,invert_q2a, y_down_ccw ^ invert_y);
			*/
			affines[nscene] = tiltToAffine(
					tilts[nscene], // double [] tilt,
					invert_q2a, // boolean invert_q2a,  // invert result affines (to match "usual")
					invert_y); // boolean invert_y)
			if (debug) {
				double [] w_min_max = SingularValueDecomposition.getMinMaxEigenValues(affines[nscene]);
				aff_tilts[nscene] = w_min_max[1]/w_min_max[0]-1.0; // >=0.0
			}
		}
//		double [][] iaffines0 = matInverse2x2(affines[0]);
		double [][] affine_diff =  matMult2x2(affines[1], matInverse2x2(affines[0]));
		if (invert_order) {
			 affine_diff =         matMult2x2(matInverse2x2(affines[0]), affines[1]);
		}
		if (debug) {		
			double [] w_min_max_diff = SingularValueDecomposition.getMinMaxEigenValues(affine_diff);
			double aff_tilt_diff = w_min_max_diff[1]/w_min_max_diff[0] - 1.0; // >=0.0
			System.out.println(String.format(" tilt0=%12.9f, tilt1=%12.9f, tilt_diff=%12.9f",aff_tilts[0], aff_tilts[1], aff_tilt_diff));
		}
		/*
		SingularValueDecomposition[] svds = new SingularValueDecomposition[2];
		for (int nscene = 0; nscene < 2; nscene++) {
			svds[nscene] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[nscene], y_down_ccw ^ invert_y);
		}
		SingularValueDecomposition svds_diff =SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_diff, y_down_ccw);
		 */
		return affine_diff;
	}	
	
	public static double [][] tiltToAffine(
			double [] tilt,
			boolean invert_q2a,  // invert result affines (to match "usual")
			boolean invert_y){
		boolean	 y_down_ccw = true;
		double [] quat = sceneRelLocalGround(
    			tilt, // double []   txy,
    			null, // double [][] affine,
    			y_down_ccw); // boolean y_down_ccw)
		double [][] affine = quatToAffine(quat ,invert_q2a, y_down_ccw ^ invert_y);
		return affine;
	}
	
	
	
	public static double [] manualFitTilt(
			double [] tilts0,
			double [] tilts_diff,
			double [][] affine_pair_nors,
			boolean invert_q2a, // invert result affines (to match "usual")
			boolean invert_order,
			boolean invert_y) {
		boolean	 y_down_ccw = true;
		boolean use_degrees = true;
		boolean debug_tilts = true;
		double [] tilt0_delta = new double[3]; // [2] - not used
		double [] tilts0_mod = tilts0.clone();
		double [] prev_tilts=new double [3];
		double [] prev2_tilts=new double [3];
		double last_err = Double.NaN;
		double prev_err = Double.NaN;
		double prev2_err = Double.NaN;
		double err_scale = 1e6;
		while(true) {
			double last_tilt=Math.sqrt(tilt0_delta[0]*tilt0_delta[0]+tilt0_delta[1]*tilt0_delta[1]);
			double prevt_tilt=Math.sqrt(prev_tilts[0]*prev_tilts[0]+prev_tilts[1]*prev_tilts[1]);
			GenericJTabbedDialog gd = new GenericJTabbedDialog("Select tilt correction, TILT="+
					String.format("TILT=%.3f%% (%.3f%%)",100*last_tilt, 100*prevt_tilt),450,150);
			
			gd.addNumericField(String.format("tiltX (%8.5f)",tilts0[0]),  tilt0_delta[0],  6,10, String.format("was %f, %f", prev_tilts[0],prev2_tilts[0]),
					"Set correction to tilts0[0]");
			gd.addNumericField(String.format("tiltY (%8.5f)",tilts0[1]),  tilt0_delta[1],  6,10, String.format("was %f, %f", prev_tilts[1],prev2_tilts[1]),
					"Set correction to tilts0[1]");
			gd.addMessage(String.format("Last error\u00D7%.0e = %.4f (previous was %.4f, %4f)", err_scale, err_scale*last_err, err_scale*prev_err, err_scale*prev2_err));
			gd.showDialog();
			if (gd.wasCanceled()) break;
	        prev2_tilts = prev_tilts.clone();			
	        prev_tilts = tilt0_delta.clone();			
			tilt0_delta[0] = gd.getNextNumber();
			tilt0_delta[1] = gd.getNextNumber();
			double [] tilts1_mod = tilts0_mod.clone();

			for (int i = 0; i < 2; i++) {
				tilts0_mod[i] = tilts0[i]+tilt0_delta[i];
				tilts1_mod[i] += tilts_diff[i];

//				System.out.println(String.format("tilt0%d diff:%9.6f was:%9.6f delta:%9.6f final:%9.6f",
//						i, tilts_diff[i], tilts0[i], tilt0_delta[i], tilts0_mod[i]));
			}
			double tilt0_delta_len = Math.sqrt(tilt0_delta[0]*tilt0_delta[0]+tilt0_delta[1]*tilt0_delta[1]);
			double tilt0_len = Math.sqrt(tilts0_mod[0]*tilts0_mod[0]+tilts0_mod[1]*tilts0_mod[1]);
			double tilt1_len = Math.sqrt(tilts1_mod[0]*tilts1_mod[0]+tilts1_mod[1]*tilts1_mod[1]);
			double tilts_diff_len = Math.sqrt(tilts_diff[0]*tilts_diff[0]+tilts_diff[1]*tilts_diff[1]);
			double tilt01_dot = tilts0_mod[0]*tilts1_mod[0]+tilts0_mod[1]*tilts1_mod[1];
			double tilt01_acos = Math.acos(tilt01_dot/tilt0_len/tilt1_len)*180/Math.PI; 

//			double 
			System.out.println("---------------------------------------: "+
					String.format("TILT_CORR=%.3f%% (TILT0=%.3f%%, TILT1=%.3f%%, TILTS_COS=%.1f\u00B0, TILT_DIFF=%.3f%%)",
							100*tilt0_delta_len,
							100*tilt0_len,
							100*tilt1_len,
							tilt01_acos,
							100*tilts_diff_len));
			for (int i = 0; i < 2; i++) {
				System.out.println(String.format("tilt0%d diff:%9.6f was:%9.6f delta:%9.6f final:%9.6f",
						i, tilts_diff[i], tilts0[i], tilt0_delta[i], tilts0_mod[i]));
			}

        	double [][] affine_tilt_diff = diffAffineFromTilts(
        			tilts0_mod, // double [] tilts0,
        			tilts_diff,     // 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);
	        //affine_pair_nors
	        SingularValueDecomposition svd_affine_pair_nors =
	        		SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_pair_nors, y_down_ccw);
        	System.out.println(affinesToString(affine_pair_nors, "affine_pair_nors"));
        	System.out.println("svd_affine_pair_nors= " + svd_affine_pair_nors.toString(use_degrees));
	        System.out.println();
	        
        	System.out.println(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 =  matMult2x2(affine_tilt_diff, matInverse2x2(affine_pair_nors));
			double [] w_min_max_err = SingularValueDecomposition.getMinMaxEigenValues(adq_err);
			double aff_tilt_err = w_min_max_err[1]/w_min_max_err[0]-1.0; // >=0.0
//			System.out.println(String.format(" tilt0=%12.9f, tilt1=%12.9f, tilt_diff=%12.9f",aff_tilts[0]-1.0, aff_tilts[1]-1.0, aff_tilt_diff-1.0));

	        SingularValueDecomposition svd_adq_err=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(adq_err, y_down_ccw);
        	System.out.println(affinesToString(adq_err, "adq_err"));
	        System.out.println("svd_adq_err=   " + svd_adq_err.toString(use_degrees));
	        /*
	        double err = Math.sqrt(
	        		(adq_err[0][0]-1.0)*(adq_err[0][0]-1.0)+
	        		(adq_err[1][1]-1.0)*(adq_err[1][1]-1.0) +
	        		adq_err[0][1]*adq_err[0][1]+adq_err[1][0]*adq_err[1][0]);
	        System.out.println(String.format("err=%12.10f",err));
	        */
	        System.out.println(String.format("AFF_TILT_ERR\u00D7%.0e=%.4f (previous were %.4f, %.4f)",
	        		err_scale, err_scale*aff_tilt_err, err_scale*last_err, err_scale*prev_err));
	        System.out.println();
	        prev2_err=prev_err;
	        prev_err=last_err;
	        last_err=aff_tilt_err;
		}
		return tilts0_mod; // tilt0_delta;
	}	
	
	
	
	
	/**
	 * Remove rotation around Z-axis, keep only tilt around axis in XY plane
	 * @param quat_in input rotation that may include rotation around Z (vertical) axis
	 * @return nearest rotation around axis in XY plane
	 */
	public static double [] pureTilt(
			double [] quat_in) {
		double r0 = quat_in[0], r1 = quat_in[1], r2 = quat_in[2], r3 = quat_in[3];
		if (r0 == 0) {
			return UNIT_QUAT;
		}
		double r3_r0 = r3/r0;
		// find {s0,0,0,s3} - rotation around vertical axis so r*s will not have Z component
		double s0 = Math.sqrt(1/(1 + r3_r0*r3_r0));
		double s3 = -r3_r0*s0;
		
		double [] quat = {
				r0 * s0 - r3 * s3,
				r1 * s0 - r2 * s3,
				r1 * s3 + r2 * s0,
				r0 * s3 + r3 * s0
		};
		return quat;
	}

	/**
	 * Normalize quaterion (in-place) and return its original length to be used as scale
	 * @param quat quaternion with encoded scale, will be normalized in place
	 * @return length of the input quaternion before normalization;
	 */
	public static double normQuat(double [] quat) {
		double s2 = 0;
		for (double q:quat) {
			s2+= q*q;
		}
		double l = Math.sqrt(s2);
		for (int i = 0; i < quat.length;i++) {
			quat[i] /= l;
		}
		return l;
	}
	
	/**
	 * Convert rotation around some axis in XY plane to affine transformation
	 * @param quat rotation, quat[3]~=0 (y-up)
	 * @param stretch project to tilted plane (stretch perpendicular to rotation axis)
	 * @param make__pure_tilt rotate around vertical axis (Z) to make quat[3]=0
	 * @return [2][2] affine transform ( y-down)
	 */
	public static double [][] quatToAffine(
			double [] quat,
			boolean   stretch,
			boolean   make__pure_tilt,
			boolean   y_down_ccw){
		// TODO: use scale
		double scale = normQuat(quat);
		double y_sign = y_down_ccw?-1:1; // invert y from quaternions to affines
		if (make__pure_tilt) {
			quat = pureTilt(quat);
		}
		double ax = quat[1], ay = quat[2] * y_sign;
		double l = Math.sqrt(ax*ax+ay*ay);
		if (l == 0) {
			return UNIT_AFFINE;
		}
		ax /= l;
		ay /= l;
		double cos_theta = 2 * quat[0]*quat[0] - 1; // cos(2 * A) = 2 * cos(A)^2 -1
		double k = stretch ? (1/cos_theta) : cos_theta;
		double s = k - 1;
		double ax2 = ax*ax, ay2=ay*ay, axy = ax*ay;
		double [][] affine = {
//				{1 + s * ax * ax,       s * ax * ay}, 
//				{  - s * ax * ay ,  1 + s * ay * ay}};
///		{k*ax2 + ay2,-s*axy}, 
///		{-s*axy ,      ax2 + k*ay2}};
				{1 + s*ay2,     -s*axy}, 
				{   -s*axy  ,1 + s*ax2}};
//		{1 + s*ay2,     s*axy}, 
//		{   s*axy  ,1 + s*ax2}};
		for (int i = 0; i < 2; i++) {
			for (int j=0; j < 2; j++) {
				affine[i][j] *= scale;
			}
		}
		return affine;
	}
	
// external invert
	public static double [][] affineToQuatScaled (
			double [][] affine,
			boolean     y_down_ccw) {
		boolean invert = true;
		if (invert) {
			affine = matInverse2x2(affine);
		} else {
			affine = affine.clone(); // isolate from input
		}
//		double [] eigen_vals= SingularValueDecomposition.getMinMaxEigenValues(affine);
//		double scale =  eigen_vals[1];

		SingularValueDecomposition svd= SingularValueDecomposition.singularValueDecomposeScaleTiltBeta(
				affine,
				y_down_ccw);
		double y_sign = y_down_ccw? -1 : 1;
		double scale = svd.getMaxScale();
		matScale(affine, 1/scale);
		double rot = svd.getRotAngle(); // rot sign should correspond Y-up (quaterions)
		double chalfrot = Math.cos(rot/2); 
		double shalfrot = Math.sin(rot/2); 
		double [] qrot = {chalfrot,0,0,shalfrot}; // rotation around vertical axis
		
		double [][] invRotMatrix = SingularValueDecomposition.rotMatrix(-y_sign*rot); // undo rotation from affine
		affine = matMult2x2(affine, invRotMatrix); // check symmetrical diagonal
		
		// undo rotation
		
		// Now beta,rot correspond to Y - up
		// rotation after rot is -beta (rot(beta)*W*rot(-beta)*rot(rot)
		boolean stretch = false;
		double cmbeta = stretch ? Math.cos(-svd.beta) : (Math.sin(-svd.beta));
		double smbeta = stretch ? Math.sin(-svd.beta) : (-Math.cos(-svd.beta)); // TODO: check sign !
		double tiltAngle = svd.getTiltAngle(); // >0
		double ctilt = Math.cos(tiltAngle/2);
		double stilt = Math.sin(tiltAngle/2);
		double [] q_plus =  {ctilt,  stilt*cmbeta,  stilt*smbeta, 0};
		double [] q_minus = {ctilt, -stilt*cmbeta, -stilt*smbeta, 0};
///		double [][] quats_pm = {scale(multiply(q_plus, qrot), scale),scale(multiply(q_minus, qrot), scale)};
		double [][] quats_pm = {scale(multiply(qrot,q_plus), scale),scale(multiply(qrot, q_minus), scale)};
		return quats_pm;
	}
	
	
	
	public static double [][] quatToAffine(
			double [] quat,
			boolean   invert,
			boolean   y_down_ccw){
		/*
The product of two quaternions:
t = rs
(t0, t1, t2, t3) = (r0, r1, r2, r3) * (s0, s1, s2, s3)

t0 = (r0s0 - r1s1 - r2s2 - r3s3)
t1 = (r0s1 + r1s0 - r2s3 + r3s2)
t2 = (r0s2 + r1s3 + r2s0 - r3s1)
t3 = (r0s3 - r1s2 + r2s1 + r3s0)
p = (0, x, y, z)
p'= inv(q) * p *q for active rotation (we'll need an inverse to get source pixel coordinates x,y

point (z==0), starting with map coordinates

p = (0, x, y, 0)

pq0 = ( - r1s1 - r2s2 ) = (-x * q[1] - y * q[2])
pq1 = ( + r1s0 - r2s3 ) = ( x * q[0] - y * q[3])
pq2 = ( + r1s3 + r2s0 ) = ( x * q[3] + y * q[0])
pq3 = ( - r1s2 + r2s1 ) = (-x * q[2] + y * q[1])

~q=[q0,-q1,-q2,-q3]
~q*p*q0 = (q0pq0 +  q1pq1 +  q2pq2 + -q3pq3)  = (q0pq0 + q1pq1 + q2pq2 + q3pq3)
~q*p*q1 = (q0pq1 + -q1pq0 - -q2pq3 + -q3pq2)  = (q0pq1 - q1pq0 + q2pq3 - q3pq2)
~q*p*q2 = (q0pq2 + -q1pq3 + -q2pq0 - -q3pq1)  = (q0pq2 - q1pq3 - q2pq0 + q3pq1)
~q*p*q3 = (q0pq3 - -q1pq2 + -q2pq1 + -q3pq0)  = (q0pq3 + q1pq2 - q2pq1 - q3pq0)


~q*p*q0 = (q0pq0 + q1pq1 + q2pq2 + q3pq3)
~q*p*q1 = (q0pq1 - q1pq0 + q2pq3 - q3pq2)
~q*p*q2 = (q0pq2 - q1pq3 - q2pq0 + q3pq1)
~q*p*q3 = (q0pq3 + q1pq2 - q2pq1 - q3pq0)

~q*p*q0 = q[0]*(-x * q[1] - y * q[2]) + q[1]*( x * q[0] - y * q[3]) + q[2]*( x * q[3] + y * q[0]) + q[3]*(-x * q[2] + y * q[1])
~q*p*q1 = q[0]*( x * q[0] - y * q[3]) - q[1]*(-x * q[1] - y * q[2]) + q[2]*(-x * q[2] + y * q[1]) - q[3]*( x * q[3] + y * q[0])
~q*p*q2 = q[0]*( x * q[3] + y * q[0]) - q[1]*(-x * q[2] + y * q[1]) - q[2]*(-x * q[1] - y * q[2]) + q[3]*( x * q[0] - y * q[3])
~q*p*q3 = q[0]*(-x * q[2] + y * q[1]) + q[1]*( x * q[3] + y * q[0]) - q[2]*( x * q[0] - y * q[3]) - q[3]*(-x * q[1] - y * q[2])


x1 = q[0]*( x * q[0] - y * q[3]) - q[1]*(-x * q[1] - y * q[2]) + q[2]*(-x * q[2] + y * q[1]) - q[3]*( x * q[3] + y * q[0])
y1 = q[0]*( x * q[3] + y * q[0]) - q[1]*(-x * q[2] + y * q[1]) - q[2]*(-x * q[1] - y * q[2]) + q[3]*( x * q[0] - y * q[3])

x1 = x* (q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3]) + y* (-q[0]*q[3] + q[1]*q[2] + q[2]*q[1] - q[3]*q[0])
y1 = x* (q[0]*q[3] + q[1]*q[2] + q[2]*q[1] + q[3]*q[0]) + y* ( q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3])

x1 = x* (q00 + q11 - q22 - q33) + y* (-q03 + q12 + q12 - q03)
y1 = x* (q03 + q12 + q12 + q03) + y* ( q00 - q11 + q22 - q33)

x1 = x* (q00 + q11 - q22 - q33) + y*2*(q12 - q03)
y1 = x*2*(q03 + q12)            + y* ( q00 - q11 + q22 - q33)

where:
q00 = q[0]*q[0]
q11 = q[1]*q[1]
q22 = q[1]*q[1]
q33 = q[1]*q[1]
q03 = q[0]*q[3]
q12 = q[1]*q[2]


q00 + q11 - q22 - q33 = a00
q12 - q03 =             a01/2
q03 + q12 =             a10/2
q00 - q11 + q22 - q33 = a11

q00 - q33 =            (a00+a11)/2
q11 - q22 =            (a00-a11)/2
q12 - q03 =             a01/2
q03 + q12 =             a10/2

q12 =       (a10+a01)/2
q03 =       (a10-a01)/2
q00 - q33 = (a00+a11)/2
q11 - q22 = (a00-a11)/2

q12 =       c0
q03 =       c1
q00 - q33 = c2
q11 - q22 = c3

		 */
		quat = quat.clone(); // not to modify original
		double scale = normQuat(quat);
		double y_sign = y_down_ccw?-1:1; // invert y from quaternions to affines
//		double scale_y = scale * (y_down_ccw?-1:1); // invert y from quaternions to affines
		double q00 = quat[0]*quat[0];
		double q11 = quat[1]*quat[1];
		double q22 = quat[2]*quat[2];
		double q33 = quat[3]*quat[3];
		double q03 = quat[0]*quat[3];
		double q12 = quat[1]*quat[2]*y_sign;
		
		double [][] affine = {
				{scale*(q00 + q11 - q22 - q33), scale*2*(q12 - q03)},
				{scale*2*(q03 + q12),           scale*(q00 - q11 + q22 - q33)}};
		if (invert) {
			affine = matInverse2x2(affine);
		}
		return affine;
	}
	/**
	 * Restore quaternion (rotation+scale) from affine transform (only [2][2] is used, ok to have [2][3] input
	 * As there are 2 solutions, both are provided in the output. As the
	 * input linear transformation matrix converts ground coordinates to source
	 * image coordinates, the scale in the tilt direction is > than scale in the
	 * perpendicular direction (tilt axis).
	 * @param affine affine transform, only [2][2] top-left subarray is used
	 * @param y_down_ccw if true, affine corresponds to to y-down (image) coordinate system
	 * @return a pair of quaternions including scale (sqrt(q0^2+q1^2+q2^2+q3^2)
	 */
	public static double [][] affineToQuatScaled (
			double [][] affine,
			boolean     stretch,// false
			boolean     y_down_ccw) {
		affine = matInverse2x2(affine);
		
		// TODO: why have to use gamma for direction?
		SingularValueDecomposition svd= SingularValueDecomposition.singularValueDecomposeScaleTiltBeta(
				affine,
				y_down_ccw);
//		SingularValueDecomposition svd= SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(
//				affine,
//				y_down_ccw);
//		double scale = stretch? svd.getMinScale(): (1/svd.getMinScale());
		double scale = stretch? svd.getMinScale(): svd.getMaxScale();
		double rot = svd.getRotAngle(); // TODO: check sign !
		// Now beta,rot correspond to Y - up
		double chalfrot = Math.cos(rot/2); 
		double shalfrot = Math.sin(rot/2); 
		double [] qrot = {chalfrot,0,0,shalfrot}; // rotation around vertical axis
		// rotation after rot is -beta (rot(beta)*W*rot(-beta)*rot(rot)
//		double cmbeta = stretch ? Math.cos(-svd.beta) : (Math.sin(-svd.beta));
//		double smbeta = stretch ? Math.sin(-svd.beta) : (-Math.cos(-svd.beta)); // TODO: check sign !
		double mbeta= svd.gamma; // -svd.beta
		double cmbeta = stretch ? Math.cos(mbeta) : (Math.sin(mbeta));
		double smbeta = stretch ? Math.sin(mbeta) : (-Math.cos(mbeta)); // TODO: check sign !
		double tiltAngle = svd.getTiltAngle(); // >0
		double ctilt = Math.cos(tiltAngle/2);
		double stilt = Math.sin(tiltAngle/2);
		double [] q_plus =  {ctilt,  stilt*cmbeta,  stilt*smbeta, 0};
		double [] q_minus = {ctilt, -stilt*cmbeta, -stilt*smbeta, 0};
///		double [][] quats_pm = {scale(multiply(q_plus, qrot), scale),scale(multiply(q_minus, qrot), scale)};
		double [][] quats_pm = {scale(multiply(qrot,q_plus), scale),scale(multiply(qrot, q_minus), scale)};
		return quats_pm;
	}
	
	public static double [][] matMult (
			double [][] m1,
			double [][] m2){
		if (m1[0].length != m2.length) {
			System.out.println("matMul(): m1[0].length != m2.length ("+m1[0].length+" !="+m2.length+")");
			return null;
		}
		double [][] m = new double[m1.length][m2[0].length];
		for (int i = 0; i < m.length; i++) {
			for (int j = 0; j < m[i].length; j++) {
				for (int k = 0; k < m1.length; k++) {
					m[i][j] += m1[i][k]*m2[k][j];
				}
			}
		}
		return m;
	}

	public static double [][] matAdd2x2 (
			double [][] m1,
			double [][] m2){
		double [][] m = new double[m1.length][m2[0].length];
		for (int i = 0; i < 2; i++) {
			for (int j = 0; j < 2; j++) {
				m[i][j] = m1[i][j] + m2[i][j];
			}
		}
		return m;
	}
	
	
	public static double [][] matMult2x2 (
			double [][] m1,
			double [][] m2){
		double [][] m = new double[m1.length][m2[0].length];
		for (int i = 0; i < 2; i++) {
			for (int j = 0; j < 2; j++) {
				for (int k = 0; k < 2; k++) {
					m[i][j] += m1[i][k]*m2[k][j];
				}
			}
		}
		return m;
	}
	
	public static double [][] dmatMult2x2 (
			double [][] dm1,
			double [][] m1,
			double [][] dm2,
			double [][] m2){
		double [][] dm = new double[m1.length][m2[0].length];
		for (int i = 0; i < 2; i++) {
			for (int j = 0; j < 2; j++) {
				for (int k = 0; k < 2; k++) {
					dm[i][j] += dm1[i][k]*m2[k][j] + m1[i][k]*dm2[k][j];
				}
			}
		}
		return dm;
	}
	
	
	
	public static double [] matMult (
			double [][] m1,
			double []   v){
		if (m1[0].length != v.length) {
			System.out.println("matMul(): m1[0].length != v.length ("+m1[0].length+" !="+v.length+")");
			return null;
		}
		double [] v2 = new double[v.length];
		for (int i = 0; i < m1.length; i++) {
				for (int k = 0; k < m1.length; k++) {
					v2[i] += m1[i][k]*v[k];
				}
		}
		return v2;
	}
	
	public static void matScale (
			double [][] a,
			double      scale){
		for (int i = 0; i < 2; i++) {
			for (int j=0; j < 2; j++) {
				a[i][j] *= scale;
			}
		}
	}
	
	public static double [][] matInverse2x2(
			double [][] a) {
		double idet = 1.0/(a[0][0] * a[1][1] - a[0][1] * a[1][0]);
		return new double [][] {
			{ idet*a[1][1], -idet*a[0][1]},
			{-idet*a[1][0],  idet*a[0][0]}};
	}

	public static double [][] dmatInverse2x2(
			double [][] da,
			double [][] a) {
		double det = (a[0][0] * a[1][1] - a[0][1] * a[1][0]);
		double idet = 1.0/det;
		double ddet = a[0][0] * da[1][1]+da[0][0] * a[1][1] - a[0][1] * da[1][0]- da[0][1] * a[1][0];
		double didet = -1/det/det*ddet;
		return new double [][] {
			{ didet*a[1][1] + idet*da[1][1], -didet*a[0][1] - idet*da[0][1]},
			{-didet*a[1][0] - idet*da[1][0],  didet*a[0][0] + idet*da[0][0]}};
	}

	
	/**
	 * c = cos (phi), s = sin (phi), w1 = 1, w2 = k, k1=1-k,
	 * tx, ty(down), t= sqrt(tx*tx+ty*ty), w2= 1/sqrt(1+tx*tx+ty*ty)
	 * c = tx/t, s=ty/t, t2=t*t
	 * | c, -s |   | w1, 0 |   |  c, s |   | 1-(1-k)*s^2, s*c*(1-k)  |          | t2 - k1*ty*ty, k1*tx*ty |
	 * |       | * |       | * |       | = |                         | = 1/t2 * |                         |
	 * | s,  c |   | 0,  w2|   | -s, c |   | s*c*(1-k),  1-(1-k)*c^2 |          | k1*tx*ty,  t2 -k1*tx*tx |
	 * 
	 */
	public static double [][][] tiltToAffineAndDerivatives (
			double [] txy){
		double sigma = 2e-4;
		double sigma2 = sigma*sigma;
		double tx = txy[0],ty=txy[1];
		if (tx*tx < sigma2) tx = sigma; 
		if (ty*ty < sigma2) ty = sigma; 
		double t2 = tx*tx + ty*ty;
		double t = Math.sqrt(t2);
		double k = 1/Math.sqrt(1+t2);
		double c = tx/t, s=ty/t, c2 = c*c, s2=s*s, sc=s*c, k1=1-k;
		double d = k/(1+t2); //1/sqrt(1+tx^2+ty^2)^3
		double dk1_dtx = tx*d;
		double dk1_dty = ty*d;
		double dc2_dtxy=2*txy[0]*txy[1]/(t2*t2);
		double dc2_dtx=dc2_dtxy * txy[1];
		double ds2_dty=dc2_dtxy * txy[0];
		double ds2_dtx= -dc2_dtx; 
		double dc2_dty= -ds2_dty; 
		double dsc_dtxy = (txy[0]*txy[0] - txy[1]*txy[1])/(t2*t2);
		double dsc_dtx =  dsc_dtxy * txy[1]; 
		double dsc_dty = -dsc_dtxy * txy[0]; 

		double [][][] a = {
				{   // a
					{1-k1*s2, k1*sc},
					{k1*sc,  1-k1*c2}
				},

				{   // da/dtx
					{-dk1_dtx * s2 - k1 * ds2_dtx,  dk1_dtx * sc + k1 * dsc_dtx},
					{ dk1_dtx * sc + k1 * dsc_dtx, -dk1_dtx * c2 - k1 * dc2_dtx} 

				},

				{   // da/dty
					{-dk1_dty * s2 - k1 * ds2_dty,  dk1_dty * sc + k1 * dsc_dty},
					{ dk1_dty * sc + k1 * dsc_dty, -dk1_dty * c2 - k1 * dc2_dty} 

				}
		};
		return a;
	}
	
	public static double [][] tiltToAffine (
			double [] txy){
		double t2 = txy[0]*txy[0] + txy[1]*txy[1];
		double t = Math.sqrt(t2);
		double k = 1/Math.sqrt(1+t2);
		double c = (t==0)?1:(txy[0]/t), s=(t==0)?0:(txy[1]/t), c2 = c*c, s2=s*s, sc=s*c, k1=1-k;
		return  new double [][] {   // a
				{1-k1*s2, k1*sc},
				{k1*sc,  1-k1*c2}};
	}
	
	
	/**
	 * Get scene affine and derivatives from ERS parameters (txy) and tilt (tiltX, tiltY) 
	 * @param txy  tilt defining ERS parameters 
	 * @param tilt actual orientation tilt (no rotation) 
	 * @return
	 */
	public static double [][][] getERSAffineAndDerivatives (
			double [] txy,
			double [][] a_tilt){
		double [][][] ers_with_derivs =  tiltToAffineAndDerivatives (
				txy); // double [] txy);
		double [][][] a = new double [ers_with_derivs.length][][];
		for (int i = 0; i < ers_with_derivs.length; i++) {
			a[i] = matMult2x2(ers_with_derivs[i], a_tilt);
		}
		return a;
	}
	
	
	/**
	 * Get affine diference and its derivatives (by phi-s and k-s of the ERS affine transforms) for a pair of scenes
	 * @param txy  a pair of ERS tilts (each as tiltx, tilty) 
	 * @param tilts a pair of the scene tilts 
	 * @param aff_pair a differential affine from the image comparison, should have rotation and scale removed 
	 * @param invert_q2a invert scene affines (tmp debug feature)
	 * @param invert_y invert direction of the y axis (tmp debug feature)
	 * @return [5] error affine transform (and derivatives by 4 parameters) - difference between tilt/ers calculated
	 *          transform and the one from image comparison
	 */
	public static double [][][] affineDiffAndDerivatives(
		double [][]   txy,   // [scene][direction]
		double [][][] a_tilts,
		double [][]   iaff_pair, // affine pair inversed
		boolean       invert_q2a){  // invert result affines (to match "usual")
		double [][][][] affs = new double [2][][][];
		double   [][][] aff =  new double [3][][];
		for (int nscene = 0; nscene < affs.length; nscene++) {
			affs[nscene] = getERSAffineAndDerivatives (
					txy[nscene],        // double [] txy,
					a_tilts[nscene]);   // double [] tilt,
			if (invert_q2a) {
				affs[nscene][0] = matInverse2x2(affs[nscene][0]);
				for (int i = 1; i < affs[nscene].length; i++) { // derivatives of inverse affine matrix
					affs[nscene][i] = dmatInverse2x2(
							affs[nscene][i],    // double [][] da,
							affs[nscene][0]);   // double [][] a) 
				}
			}
		}
		double [][][] iaff0 = new double [aff.length][][]; // d_phi, d_k
		iaff0[0] = matInverse2x2(affs[0][0]);    // inverse affine matrix aff0
		for (int i = 1; i < iaff0.length; i++) { // derivatives of inverse affine matrix
			iaff0[i] = dmatInverse2x2(
					affs[0][i],  // double [][] da,
					affs[0][0]); // double [][] a) 
		}
		// Now iaffs0 is inverse of affs[0] and 2 of its derivatives (by tx and ty)
		double [][][] aff_diff = new double [5][][]; //aff_err, d_aff_err/d_tx0, d_aff_err/d_y0, d_aff_err/d_tx1, d_aff_err/d_y1
		aff_diff[0] = matMult2x2(affs[1][0], iaff0[0]); // aff_err
		aff_diff[1] = matMult2x2(affs[1][0], iaff0[1]); // d_aff_err/d_tx0
		aff_diff[2] = matMult2x2(affs[1][0], iaff0[2]); // d_aff_err/d_ty0
		aff_diff[3] = matMult2x2(affs[1][1], iaff0[0]); // d_aff_err/d_tx1
		aff_diff[4] = matMult2x2(affs[1][2], iaff0[0]); // d_aff_err/d_ty1
		
		
		/*
		for (int i = 1; i < aff.length; i++) {
			aff_diff[i] = dmatMult2x2 (
					affs[1][i], // double [][] dm1,
					affs[1][0], // double [][] m1,
					iaff0[i],   // double [][] dm2,
					iaff0[0]);  // double [][] m2)
		}
		*/
		double [][][] aff_err = new double [aff_diff.length][][]; //aff_err, d_aff_err/d_phi, d_aff_err/d_k
//		double [][] iaff_pair = matInverse2x2(aff_pair);
		for (int i = 0; i < aff_err.length; i++) {
			aff_err[i] = matMult2x2(aff_diff[i],iaff_pair);
		}
		return aff_err;
	}
	
	
	/**
	 * c = cos (phi), s = sin (phi), w, k=1-w,
	 * tx, ty(down), t= sqrt(tx*tx+ty*ty), w2= 1/sqrt(1+tx*tx+ty*ty)
	 * c = tx/t, s=ty/t, t2=t*t
	 * | c, -s |   | 1,  0 |   |  c, s |   | 1-(1-w)*s*s, s*c*(1-w)  |
	 * |       | * |       | * |       | = |                         |
	 * | s,  c |   | 0,  w |   | -s, c |   | s*c*(1-w),  1-(1-w)*c*c |
	 * 
	 */
	public static double [][][] angleWToAffineAndDerivatives (
			double beta,
			double w){ // w<1
		double c = Math.cos(beta), s = Math.sin(beta), c2 = c * c, s2 = s*s, sc= s*c,k = 1-w;
		

		double [][][] a = {
				{   // a
					{1-k*s2, k*sc},
					{k*sc,  1-k*c2}
				},

				{   // da/dbeta
					{-k*2*sc, k*(c2-s2)},
					{k*(c2-s2), k*2*sc}

				},

				{   // da/dw
					{-s2, sc},
					{sc, -c2} 

				}
		};
		return a;
	}
	
	
	/**
	 * Get scene affine and derivatives from ERS parameters (txy) and tilt (tiltX, tiltY) 
	 * @param beta
	 * @param a_tilt actual orientation affine (no rotation) 
	 * @return
	 */
	public static double [][][] getERSAffineAndDerivatives (
			double      beta,
			double      w, // w<1
			double [][] a_tilt){
		double [][][] ers_with_derivs =  angleWToAffineAndDerivatives (
				beta, // double beta,
				w);   // double w){ // w<1
		double [][][] a = new double [ers_with_derivs.length][][];
		for (int i = 0; i < ers_with_derivs.length; i++) {
			a[i] = matMult2x2(ers_with_derivs[i], a_tilt);
		}
		return a;
	}
	
	/**
	 * Get affine diference and its derivatives (by phi-s and k-s of the ERS affine transforms) for a pair of scenes
	 * @param txy  a pair of ERS tilts (each as tiltx, tilty) 
	 * @param tilts a pair of the scene tilts 
	 * @param aff_pair a differential affine from the image comparison, should have rotation and scale removed 
	 * @param invert_q2a invert scene affines (tmp debug feature)
	 * @param invert_y invert direction of the y axis (tmp debug feature)
	 * @return [5] error affine transform (and derivatives by 4 parameters) - difference between tilt/ers calculated
	 *          transform and the one from image comparison
	 */
	public static double [][][] affineDiffAndDerivatives(
			double []   betas,
			double []   ws, // w<1
		double [][][] a_tilts,
		double [][]   iaff_pair, // affine pair inversed
		boolean       invert_q2a){  // invert result affines (to match "usual")
		double [][][][] affs = new double [2][][][];
		double   [][][] aff =  new double [3][][];
		for (int nscene = 0; nscene < affs.length; nscene++) {
			affs[nscene] = getERSAffineAndDerivatives (
					betas[nscene],      // double      beta,
					ws[nscene],         // double      w, // w<1
					a_tilts[nscene]);   // double [] tilt,
			if (invert_q2a) {
				affs[nscene][0] = matInverse2x2(affs[nscene][0]);
				for (int i = 1; i < affs[nscene].length; i++) { // derivatives of inverse affine matrix
					affs[nscene][i] = dmatInverse2x2(
							affs[nscene][i],    // double [][] da,
							affs[nscene][0]);   // double [][] a) 
				}
			}
		}
		double [][][] iaff0 = new double [aff.length][][]; // d_phi, d_k
		iaff0[0] = matInverse2x2(affs[0][0]);    // inverse affine matrix aff0
		for (int i = 1; i < iaff0.length; i++) { // derivatives of inverse affine matrix
			iaff0[i] = dmatInverse2x2(
					affs[0][i],  // double [][] da,
					affs[0][0]); // double [][] a) 
		}
		// Now iaffs0 is inverse of affs[0] and 2 of its derivatives (by tx and ty)
		double [][][] aff_diff = new double [5][][]; //aff_err, d_aff_err/d_tx0, d_aff_err/d_y0, d_aff_err/d_tx1, d_aff_err/d_y1
		aff_diff[0] = matMult2x2(affs[1][0], iaff0[0]); // aff_err
		aff_diff[1] = matMult2x2(affs[1][0], iaff0[1]); // d_aff_err/d_tx0
		aff_diff[2] = matMult2x2(affs[1][0], iaff0[2]); // d_aff_err/d_ty0
		aff_diff[3] = matMult2x2(affs[1][1], iaff0[0]); // d_aff_err/d_tx1
		aff_diff[4] = matMult2x2(affs[1][2], iaff0[0]); // d_aff_err/d_ty1
		double [][][] aff_err = new double [aff_diff.length][][]; //aff_err, d_aff_err/d_phi, d_aff_err/d_k
		for (int i = 0; i < aff_err.length; i++) {
			aff_err[i] = matMult2x2(aff_diff[i],iaff_pair);
		}
		return aff_err;
	}
	
	

	/**
	 * Generate affine transform corresponding to arbitrary stretch from 2 parameters
	 * centered around zero that have lower power than actual tilts and so 
	 * "better" derivatives for LMA near (0,0). They are still all 0, but
	 * minimizing each of them will add non-zero derivatives	
	 * @param xy pair of X/y parameters
	 * @return affine [2][2] matrix and 2 derivatives by X and Y
	 */
	
	public static double [][][] pseudoTiltToAffineAndDerivatives0 (
			double [] xy){
		double x = xy[0],y=xy[1];

		double [][][] a = {
				{   // a
					{1-y*y, x*y},
					{x*y,  1-x*x}
				},

				{   // da/dx
					{0,   y},
					{y, -2*x} 

				},

				{   // da/dy
					{-2*y, x},
					{x,    0} 

				}
		};
		return a;
	}
	/**
	 * Different input to improve derivatives, based on dual angle
	 * Starting with SVD angle and a (1,w) pair of singular values:
	 * c = cos (phi), s = sin (phi), w, k=1-w,
	 * tx, ty(down), t= sqrt(tx*tx+ty*ty), w2= 1/sqrt(1+tx*tx+ty*ty)
	 * c = tx/t, s=ty/t, t2=t*t
	 * | c, -s |   | 1,  0 |   |  c, s |   | 1-k*s*s, s*c*k  |
	 * |       | * |       | * |       | = |                 |
	 * | s,  c |   | 0,  w |   | -s, c |   | s*c*k,  1-k*c*c |
	 * converted to dual angle
	 * x = k * cos(2*phi)/2, y = k*sin(2*phi)/2, k/2 = sqrt(x*x + y*y)
	 * r = k/2
	 *   
	 * | 1-k*s*s, s*c*k  |   | (1-r) + x,        y |
	 * |                 | = |                     |
	 * | s*c*k,  1-k*c*c |   | y,         (1-r) -x |
	 * 
	 * Simplification (by the expense of changing w1, w2 to 1 +/- sqrt (x*x+y*y),
	 * gamma = -beta = atan2(y,x)/2:
	 * | 1 + x,   y |        | 1,  0 |         | 0,  1 |
	 * |            |, d/dx= |       |, d/dy = |       |
	 * |     y, 1-x |        | 0, -1 |         | 1,  0 |
	 * 
	 * @param xy - a pair of pseudo-coordinates definining scaled tilt affine transformation.
	 * @return an affine transformation as 2x2 array and two of its derivatives by x and y,
	 *         combined into [3][2][2] array. 
	 */
	
	public static double [][][] pseudoTiltToAffineAndDerivatives (
			double [] xy){
		double x = xy[0],y=xy[1];
		double [][][] a = {
				{   // a
					{1+x,  y},
					{y,  1-x}
				},

				{   // da/dx
					{1,   0},
					{0, - 1} 
				},

				{   // da/dy
					{0,   1},
					{1,   0} 

				}
		};
		return a;
	}

	public static double [][] pseudoTiltToAffine(
			double [] xy){
		double x = xy[0],y=xy[1];
		double [][] a =
			{   // a
					{1+x,  y},
					{y,  1-x}
			};
		return a;
	}
	
	
	
	
	/**
	 * Get scene affine and derivatives from ERS parameters (txy) and tilt (tiltX, tiltY) 
	 * @param xy  pseudo-tilt defining ERS parameters 
	 * @param tilt actual orientation tilt (no rotation) 
	 * @return
	 */
	public static double [][][] pseudoERSAffineAndDerivatives (
			double [] xy,
			double [][] a_tilt){
		double [][][] ers_with_derivs =  pseudoTiltToAffineAndDerivatives (
				xy); // double [] xy);
		double [][][] a = new double [ers_with_derivs.length][][];
		for (int i = 0; i < ers_with_derivs.length; i++) {
			a[i] = matMult2x2(ers_with_derivs[i], a_tilt);
		}
		return a;
	}
	
	
	/**
	 * Get affine diference and its derivatives (by phi-s and k-s of the ERS affine transforms) for a pair of scenes
	 * @param xy  a pair of ERS pseudo-tilts (each as tiltx, tilty) 
	 * @param tilts a pair of the scene tilts 
	 * @param aff_pair a differential affine from the image comparison, should have rotation and scale removed 
	 * @param invert_q2a invert scene affines (tmp debug feature)
	 * @param invert_y invert direction of the y axis (tmp debug feature)
	 * @return [5] error affine transform (and derivatives by 4 parameters) - difference between tilt/ers calculated
	 *          transform and the one from image comparison
	 */
	public static double [][][] pseudoAffineDiffAndDerivatives(
		double [][]   xy,   // [scene][direction]
		double [][][] a_tilts,
		double [][]   iaff_pair, // affine pair inversed
		boolean       invert_q2a){  // invert result affines (to match "usual")
		double [][][][] affs = new double [2][][][];
		double   [][][] aff =  new double [3][][];
		for (int nscene = 0; nscene < affs.length; nscene++) {
			affs[nscene] = pseudoERSAffineAndDerivatives (
					xy[nscene],        // double [] xy,
					a_tilts[nscene]);   // double [] tilt,
			if (invert_q2a) {
				affs[nscene][0] = matInverse2x2(affs[nscene][0]);
				for (int i = 1; i < affs[nscene].length; i++) { // derivatives of inverse affine matrix
					affs[nscene][i] = dmatInverse2x2(
							affs[nscene][i],    // double [][] da,
							affs[nscene][0]);   // double [][] a) 
				}
			}
		}
		double [][][] iaff0 = new double [aff.length][][]; // d_phi, d_k
		iaff0[0] = matInverse2x2(affs[0][0]);    // inverse affine matrix aff0
		for (int i = 1; i < iaff0.length; i++) { // derivatives of inverse affine matrix
			iaff0[i] = dmatInverse2x2(
					affs[0][i],  // double [][] da,
					affs[0][0]); // double [][] a) 
		}
		// Now iaffs0 is inverse of affs[0] and 2 of its derivatives (by tx and ty)
		double [][][] aff_diff = new double [5][][]; //aff_err, d_aff_err/d_tx0, d_aff_err/d_y0, d_aff_err/d_tx1, d_aff_err/d_y1
		aff_diff[0] = matMult2x2(affs[1][0], iaff0[0]); // aff_err
		aff_diff[1] = matMult2x2(affs[1][0], iaff0[1]); // d_aff_err/d_tx0
		aff_diff[2] = matMult2x2(affs[1][0], iaff0[2]); // d_aff_err/d_ty0
		aff_diff[3] = matMult2x2(affs[1][1], iaff0[0]); // d_aff_err/d_tx1
		aff_diff[4] = matMult2x2(affs[1][2], iaff0[0]); // d_aff_err/d_ty1
		double [][][] aff_err = new double [aff_diff.length][][]; //aff_err, d_aff_err/d_phi, d_aff_err/d_k
		for (int i = 0; i < aff_err.length; i++) {
			aff_err[i] = matMult2x2(aff_diff[i],iaff_pair);
		}
		return aff_err;
	}

	public static void testPseudoAffineDiffAndDerivatives(
			double [][]   xy,   // [scene][direction]
			double [][][] a_tilts,
			double [][]   iaff_pair, // affine pair inversed
			boolean       invert_q2a){  // invert result affines (to match "usual")
		double        delta= 1e-8;
		/*
		xy = new double [][] {
			{xy[0][0]-0.1,xy[0][1]+0.1},
			{xy[1][0]+0.1,xy[1][1]+0.1}
		};

		a_tilts = new double[][][] {
			{
				{a_tilts[0][0][0],     a_tilts[0][0][1]-0.01},
				{a_tilts[0][1][0]-0.01,a_tilts[0][1][1]}
			},
			{
				{a_tilts[1][0][0],     a_tilts[1][0][1]-0.1},
				{a_tilts[1][1][0]-0.1, a_tilts[1][1][1]}
			}
		};

		iaff_pair = new double [][]  {
			{iaff_pair[0][0],    iaff_pair[0][1]+0.1},
			{iaff_pair[1][0]+0.1,iaff_pair[1][1]}
		};

		 */
		double [][] xy_m = new double [][] {
			{xy[0][0],xy[0][1]},
			{xy[1][0],xy[1][1]}
		};

		double [][][] a_tilts_m= new double[][][] {
			{
				{a_tilts[0][0][0],a_tilts[0][0][1]},
				{a_tilts[0][1][0],a_tilts[0][1][1]}
			},
			{
				{a_tilts[1][0][0], a_tilts[1][0][1]},
				{a_tilts[1][1][0], a_tilts[1][1][1]}
			}
		};

		double [][] iaff_pair_m = new double [][]  {
			{iaff_pair[0][0],iaff_pair[0][1]},
			{iaff_pair[1][0],iaff_pair[1][1]}
		};
		testPseudoAffineDiffAndDerivatives(
				xy_m,          // double [][]   xy,   // [scene][direction]
				a_tilts_m,     // double [][][] a_tilts,
				iaff_pair_m,   // double [][]   iaff_pair, // affine pair inversed
				invert_q2a,    // boolean       invert_q2a,  // invert result affines (to match "usual")
				delta);        // double        delta);
	}

	public static void testPseudoAffineDiffAndDerivatives(
			double [][]   xy,   // [scene][direction]
			double [][][] a_tilts,
			double [][]   iaff_pair, // affine pair inversed
			boolean       invert_q2a,  // invert result affines (to match "usual")
			double        delta) {
		while (xy[0][0] < 1) {
			double [][][] aff_err = QuatUtils.pseudoAffineDiffAndDerivatives( //affineDiffAndDerivatives(
					xy,                   // double [][]   txy,   // [scene][direction]
					a_tilts,              // double [][][] a_tilts,
					iaff_pair, //  double [][]   iaff_pair, // affine pair inversed
					invert_q2a); // boolean       invert_q2a)  // invert result affines (to match "usual")
			double [][] WdW = SingularValueDecomposition.getMinMaxEigenValues(
					aff_err); // double [][][] AdA)
			double [] dWdW = new double [WdW.length];
			for (int i = 0; i < WdW.length; i++) {
				dWdW[i] = WdW[i][1]-WdW[i][0];
			}
			double [][] WdW_delta = new double [WdW.length][2];
			double [] dWdW_delta = new double [WdW.length];
			for (int i = 1; i < dWdW_delta.length; i++) {
				int nscene= (i-1) / 2;
				int ynx =   (i-1) % 2;
				double [][] xy_p = new double [][] {xy[0].clone(), xy[1].clone()};
				double [][] xy_m = new double [][] {xy[0].clone(), xy[1].clone()};
				xy_p[nscene][ynx] += delta/2;
				xy_m[nscene][ynx] -= delta/2;
				double [][][] aff_err_p = QuatUtils.pseudoAffineDiffAndDerivatives( //affineDiffAndDerivatives(
						xy_p,                   // double [][]   txy,   // [scene][direction]
						a_tilts,              // double [][][] a_tilts,
						iaff_pair, //  double [][]   iaff_pair, // affine pair inversed
						invert_q2a); // boolean       invert_q2a)  // invert result affines (to match "usual")
				double [][][] aff_err_m = QuatUtils.pseudoAffineDiffAndDerivatives( //affineDiffAndDerivatives(
						xy_m,                   // double [][]   txy,   // [scene][direction]
						a_tilts,              // double [][][] a_tilts,
						iaff_pair, //  double [][]   iaff_pair, // affine pair inversed
						invert_q2a); // boolean       invert_q2a)  // invert result affines (to match "usual")
				double [][] WdW_p = SingularValueDecomposition.getMinMaxEigenValues(
						aff_err_p); // double [][][] AdA)
				double [][] WdW_m = SingularValueDecomposition.getMinMaxEigenValues(
						aff_err_m); // double [][][] AdA)
				WdW_delta[i][0] = (WdW_p[0][0]-WdW_m[0][0])/delta;
				WdW_delta[i][1] = (WdW_p[0][1]-WdW_m[0][1])/delta;
				dWdW_delta[i] = WdW_delta[i][1]-WdW_delta[i][0];
			}
			double s = 1; // 
			System.out.println(String.format("%5s\t%20s\t%20s\t%20s\t%20s",
					"mode#", "dx0", "dy0", "dx1", "dy1"));
			System.out.println(String.format("%5s\t%20.12f\t%20.12f\t%20.12f\t%20.12f",
					"deriv", s*dWdW[1], s*dWdW[2], s*dWdW[3], s*dWdW[4]));
			System.out.println(String.format("%5s\t%20.12f\t%20.12f\t%20.12f\t%20.12f",
					"delta", s*dWdW_delta[1], s*dWdW_delta[2],s*dWdW_delta[3],s*dWdW_delta[4]));
			System.out.println(String.format("%5s\t%20.12f\t%20.12f\t%20.12f\t%20.12f",
					"diff", s*(dWdW_delta[1]-dWdW[1]), s*(dWdW_delta[2]-dWdW[2]), s*(dWdW_delta[3]-dWdW[3]), s*(dWdW_delta[4]-dWdW[4])));
			System.out.println ("Change xy[0][0] >= 1.0 to exit");
			System.out.println ();
		}
		return;
	}
	
	public static void applyTo(
			final double [] q,
			final double [] in,
			final double [] out) {
		final double q0= q[0];
		final double q1= q[1];
		final double q2= q[2];
		final double q3= q[3];

		final double x = in[0];
		final double y = in[1];
		final double z = in[2];

		final double s = q1 * x + q2 * y + q3 * z;
		out[0] = 2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x;
		out[1] = 2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y;
		out[2] = 2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - z;
	}

	public static void applyTo(
			final double    k, // scale
			final double [] q,
			final double [] in,
			final double [] out) {
		final double q0= q[0];
		final double q1= q[1];
		final double q2= q[2];
		final double q3= q[3];

		final double x = in[0];
		final double y = in[1];
		final double z = in[2];

		final double s = q1 * x + q2 * y + q3 * z;
		out[0] = k * (2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x);
		out[1] = k * (2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y);
		out[2] = k * (2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - z);
	}
	
}
