package com.elphel.imagej.orthomosaic;

public class SingularValueDecomposition {
	double beta;
	double gamma;
	double w1;
	double w2; // < w1 after singularValueDecompose
	double rot;
	double scale;  // min (w1,w2) or sqrt(w1*w2) for raw singularValueDecompose()
	double ratio;  // <=1.0, ==cos(tilt) or w2/w1 for raw singularValueDecompose()
	boolean st= false;
	boolean y_down_ccw;	
	public SingularValueDecomposition(
			double beta,
			double gamma,
			double w1,
			double w2,
			double rot) {
		this.beta = beta;
		this.gamma = gamma;
		this.w1 = w1;
		this.w2 = w2;
		this.rot = rot;
	}
	public static double [][] rotMatrix(double a){
		double c = Math.cos(a);
		double s = Math.sin(a);
		return new double [][] {{c,s},{-s,c}};
		
	}
	// A = getR1() * getW() getR2() = getR1() * getW() * getIR1() * getRot()
	public static double [][] getUnity(){
		return new double [][] {{1,0},{0,1}};
	}
	public double [][] getR1(){	return rotMatrix(beta);}
	public double [][] getR2(){	return rotMatrix(gamma);}
	public double [][] getIR1(){return rotMatrix(-beta);} // never used
	public double [][] getRot(){return rotMatrix(rot);}
	public double [][] getW(){
		return new double [][] {{w1,0},{0,w1}};
	}
	public double getTiltAngle() {
		if (ratio <= 1.0) return Math.acos(ratio);
		else return Math.acos(1/ratio);
	}
	public double getRotAngle() {return rot;}
	public double getScale() {return scale;}
	public double getAvgScale() {return Math.sqrt(w1*w2);}
	public double getMinScale() {return Math.min(w1,w2);}
	public double getMaxScale() {return Math.max(w1,w2);}
	
	// Measure of tilt - difference between singular values (they both are assumed to be ~= 1.0)
	public double getDW() {
		return Math.abs(w1-w2);
	}
	
	public static String titleString(boolean degrees) {
		SingularValueDecomposition svd= new SingularValueDecomposition(0,0,0,0,0);
		return svd.toString(degrees, 2);
	}
	public String toString(boolean degrees) {
		return toString(degrees,0);
	}
	
	/**
	 * generate String representation of SingularValueDecomposition instance
	 * @param degrees true - degrees, false - radians
	 * @param tab_mode 0 - commas, in-line names; 1 - tabs, data only; 2 tab-sepatated title string (does not start with a tab)
	 * @return formatted String 
	 */
	public String toString(boolean degrees, int tab_mode) {
//System.out.println("svd_affine_pair=    ["+svd_affine_pair.scale+  ","+svd_affine_pair.getTiltAngle()+ ","+svd_affine_pair.gamma+ ","+svd_affine_pair.rot+
// "] tilt="+(svd_affine_pair.getTiltAngle()*180/Math.PI)+ "\u00B0, dir="+(svd_affine_pair.gamma*180/Math.PI)+"\u00B0");
		String tab_title_rad = String.format("%10s\t%10s\t%10s\t%10s\t%10s\t%10s\t%10s\t%10s",
				"scale","tilt","gamma","beta","rot", "w1", "w2", "\u0394w"); // ratio");
		String tab_title_deg = String.format("%10s\t%10s\t%10s\t%10s\t%10s\t%10s\t%10s\t%10s",
				"scale","tilt\u00B0","gamma\u00B0","beta\u00B0","rot\u00B0", "w1", "w2", "\u0394w"); // ratio");
		String tab_fmt_rad = "%10.8f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.8f\t%10.8f\t%10.8f";
		String tab_fmt_deg = "%10.8f\t%10.5f\t%10.5f\t%10.5f\t%10.5f\t%10.8f\t%10.8f\t%10.8f";
		String fmt_rad = " scale=%10.8f,tilt= %10.7f, gamma=%10.7f, beta=%10.7f, rot=%10.7f, w1=%10.8f, w2=%10.8f, \u0394w=%10.8f"; // ratio=%10.8f";
		String fmt_deg = " scale=%10.8f,tilt= %10.5f\u00B0, gamma=%10.5f\u00B0, beta=%10.5f\u00B0, rot=%10.5f\u00B0, w1=%10.8f, w2=%10.8f, \u0394w=%10.8f"; // ratio=%10.8f\";
		if (tab_mode==2) {
			return degrees?tab_title_deg:tab_title_rad;
		}
		String fmt = (tab_mode==1)?(degrees ? tab_fmt_deg : tab_fmt_rad):(degrees ? fmt_deg : fmt_rad);
		double s = degrees ? (180/Math.PI):1;
//		String fmt=degrees ? fmt_deg : fmt_rad;
//		return String.format(fmt, scale, s*getTiltAngle(), s*gamma, s*beta, s*rot, w1, w2, ratio);
		return String.format(fmt, scale, s*getTiltAngle(), s*gamma, s*beta, s*rot, w1, w2, Math.abs(w1-w2));
	}
	
	
	/**
	 * Decomposing linear transform into rotations and scales. OK to use full 2x3 affine
	 * R1={{cos(beta),sin(beta)},{-sin(beta), cos(beta)}} ,
	 * non-uniform scale transform W={{w1,0}{0,w2}}, and rotation
	 * R2={{cos(gamma),sin(gamma)},{-sin(gamma), cos(gamma)}}
	 * A=R1*W*R2 using singular value decomposition -- tested
	 * https://math.stackexchange.com/questions/861674/decompose-a-2d-arbitrary-transform-into-only-scaling-and-rotation   
	 * @param A - input 2x2 matrix
	 * @return {s,beta,w,gamma,beta+gamma}
	 * A = B + C
	 * b00= b11; c00=-c01
	 * b10=-b01; c10= c01
	 */
	public static SingularValueDecomposition singularValueDecompose(
			double [][] A) {
		double a00=A[0][0],a01=A[0][1],a10=A[1][0],a11=A[1][1];
		double b00=(a00+a11)/2; // , b11 = b00;
		double c00=(a00-a11)/2; //, c11 =-c00;
		double b01=(a01-a10)/2; //, b10 =-b01;
		double c01=(a01+a10)/2; //, c10 = c01;
		double w1_p_w2_2= Math.sqrt(b00*b00+b01*b01);
		double w1_m_w2_2= Math.sqrt(c00*c00+c01*c01);
		double w1 = w1_p_w2_2 + w1_m_w2_2;
		double w2 = w1_p_w2_2 - w1_m_w2_2;
		double g_p_b = Math.atan2(b01, b00);
		double g_m_b = Math.atan2(c01, c00);
		double gamma = (g_p_b + g_m_b)/2; 
		double beta =  (g_p_b - g_m_b)/2; 
		SingularValueDecomposition svd= new SingularValueDecomposition (beta,gamma,w1,w2,g_p_b);
		svd.scale = Math.sqrt(svd.w1*svd.w2);
		svd.ratio = svd.w2/svd.w1; // <= 1.0;
		return svd;
	}

	
	public static double [][] removeTiltRotScale(
			double [][] A,
			boolean removeTilt,
			boolean removeRot,
			boolean removeScale,
			boolean removeOffset,
			boolean max_is_scale) {
		SingularValueDecomposition svd = singularValueDecompose(A);
		double [][] AR = removeTiltRotScale(
				svd, // SingularValueDecomposition svd,
				removeTilt,   // boolean removeTilt,
				removeRot,    // boolean removeRot,
				removeScale, // boolean removeScale)
				max_is_scale); // boolean max_is_scale)
		if (A[0].length < 3) {
			return AR;
		}
		A = new double [][] {A[0].clone(),A[1].clone()};
		if (removeOffset) {
			for (int i = 0; i < 2; i++) {
				A[i][2] = 0;	
			}
		}
		for (int i = 0; i < 2; i++) {
			for (int j = 0; j < 2; j++) {
				A[i][j] = AR[i][j];	
			}
		}
		return A;
	}
	
	public static double [][] removeTiltRotScale(
			SingularValueDecomposition svd,
			boolean removeTilt,
			boolean removeRot,
			boolean removeScale,
			boolean max_is_scale) { 
		double [][] R1 =  svd.getR1();
		double [][] IR1 = svd.getIR1();
//		double [][] W =   svd.getW();
		double scale = max_is_scale?Math.max(svd.w1,svd.w2):Math.min(svd.w1,svd.w2);
		double w1 =svd.w1, w2 = svd.w2;
		if (removeScale) {
			w1 /= scale;
			w2 /= scale;
			scale = 1.0;
		}
		if (removeTilt) {
			w1 = scale;
			w2 = scale;
		}
		double [][]W = {{w1,0},{0, w2}};
		double [][] R =   removeRot?   getUnity() : svd.getRot();
		double [][] A = QuatUtils.matMult2x2(QuatUtils.matMult2x2(QuatUtils.matMult2x2(R1, W), IR1), R);
		return A;
	}	
	
	public static double [] getMinMaxEigenValues(double [][] A) {
		double a00=A[0][0],a01=A[0][1],a10=A[1][0],a11=A[1][1];
		double b00=(a00+a11)/2; // , b11 = b00;
		double c00=(a00-a11)/2; //, c11 =-c00;
		double b01=(a01-a10)/2; //, b10 =-b01;
		double c01=(a01+a10)/2; //, c10 = c01;
		double w1_p_w2_2= Math.sqrt(b00*b00+b01*b01);
		double w1_m_w2_2= Math.sqrt(c00*c00+c01*c01);
		double w1 = w1_p_w2_2 + w1_m_w2_2;
		double w2 = w1_p_w2_2 - w1_m_w2_2;
		return new double[] {w2,w1}; // because w1_m_w2_2 >=0 so w1 >= w2 
//		return (w1 < w2) ? (new double[] {w1,w2}) : (new double[] {w2,w1});

	}
	
	/**
	 * Get a pair of singular values (starting with smaller) and their derivatives
	 * @param AdA   affine transform, followed by an array of affine transform derivatives
	 *              (top index - number of derivative)
	 * @return {{w2,w1}, {dw2/dx1,dw1/dx1}, {dw2/dx2,dw1/dx2}, ...}
	 */
	public static double [][] getMinMaxEigenValues(
			double [][][] AdA) {
		double a00=AdA[0][0][0],a01=AdA[0][0][1],a10=AdA[0][1][0],a11=AdA[0][1][1];
		double b00=(a00+a11)/2; //, b11 = b00;
		double c00=(a00-a11)/2; //, c11 =-c00;
		double b01=(a01-a10)/2; //, b10 =-b01;
		double c01=(a01+a10)/2; //, c10 = c01;
		double w1_p_w2_2= Math.sqrt(b00*b00+b01*b01);
		double w1_m_w2_2= Math.sqrt(c00*c00+c01*c01);
		double w1 = w1_p_w2_2 + w1_m_w2_2;
		double w2 = w1_p_w2_2 - w1_m_w2_2;
		double [][] rslt = new double [AdA.length][];
		rslt[0] = new double [] {w2,w1}; // w1=1, w2 <=1;
		for (int i = 1; i < AdA.length; i++) {
			double d_a00=AdA[i][0][0],d_a01=AdA[i][0][1],d_a10=AdA[i][1][0],d_a11=AdA[i][1][1];
			double d_b00=(d_a00+d_a11)/2; //, b11 = b00;
			double d_c00=(d_a00-d_a11)/2; //, c11 =-c00;
			double d_b01=(d_a01-d_a10)/2; //, b10 =-b01;
			double d_c01=(d_a01+d_a10)/2; //, c10 = c01;
			double d_w1_p_w2_2= (b00*d_b00 + b01*d_b01)/w1_p_w2_2;
			double d_w1_m_w2_2= (c00*d_c00 + c01*d_c01)/w1_m_w2_2;
			double d_w1 = d_w1_p_w2_2 + d_w1_m_w2_2;
			double d_w2 = d_w1_p_w2_2 - d_w1_m_w2_2;
			rslt[i] = new double [] {d_w2,d_w1};
		}
		return rslt; 
	}
	
	
	/**
	 * Use singular value decomposition and then split scaling {{w1,0},{0,w1}}
	 * into overall scaling caused by zoom != 1.0 because of altitude error
	 * and unidirectional scaling caused by tilted projection plane. 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).
	 * Matrix R1 is additionally rotated by PI/2 if needed so W={{w1,0},{0,w2}}
	 * has w2>=w1 and W={{s,0},{0,s/t}}, where t <= 1.0 and equals to cos(tilt) 
	 * 
	 * @param A - linear transformation matrix from rectified ground coordinates
	 *        to source image coordinates. OK to use 2x3 affine matrix,extra
	 *        components will be ignored. 
	 * @param y_down_ccw - positive Y is down, positive angles are CCW
	 * @return SingularValueDecomposition instance with scale and ratio (<=1) defined, gamma and beta updated
	 */

	public static SingularValueDecomposition singularValueDecomposeScaleTiltGamma(
			double [][] A,
			boolean y_down_ccw) {
		SingularValueDecomposition svd = singularValueDecompose(A);
		svd.setScaleTiltGamma(y_down_ccw);
		return svd;
	}

	public static SingularValueDecomposition singularValueDecomposeScaleTiltBeta(
			double [][] A,
			boolean y_down_ccw) {
		SingularValueDecomposition svd = singularValueDecompose(A);
		svd.setScaleTiltBeta(y_down_ccw);
		return svd;
	}
	
	
	public void setScaleTiltGamma(
			boolean y_down_ccw) {
		// For Y-down, angles are CW positive, for Y-up angles are CCW positive
		// Affines are Y-up?
		
		// considering tilt in y direction (unless long_axis), it should have higher scale
		// (and source image coordinates), while X should correspond to
		// the axis of rotation, and scale is just scale caused by error in
		// altitude.
		scale = Math.min(w1, w2); // w1 >= w2 after singularValueDecompose() !
		ratio = w1/w2; // <=1.0, ==cos(tilt)
		if (w1 > w2) { // rotate tilt by PI/2
			ratio = w2/w1;
			gamma += Math.PI/2; // start with rotation (last in matrices)
			if (gamma > Math.PI) {
				gamma -= 2* Math.PI;
			}
		}
		
		if (gamma > Math.PI/2) {
			gamma -= Math.PI;
		} else if (gamma < -Math.PI) {
			gamma += Math.PI;
		}
		
		beta = rot-gamma;
		while (beta >= Math.PI/2) {
			beta -= Math.PI;
		}
		while (beta < -Math.PI/2) {
			beta += Math.PI;
		}
		
		if (y_down_ccw) {
			gamma *= -1; 
			beta *= -1; 
			rot *= -1; // pure rotation
		}
		this.y_down_ccw = y_down_ccw;
		st = true;
	}
	
	public void setScaleTiltBeta(
			boolean y_down_ccw) {
		// For Y-down, angles are CW positive, for Y-up angles are CCW positive
		// Affines are Y-up?
		
		// considering tilt in y direction (unless long_axis), it should have higher scale
		// (and source image coordinates), while X should correspond to
		// the axis of rotation, and scale is just scale caused by error in
		// altitude.
		scale = Math.min(w1, w2);
		ratio = w1/w2; // <=1.0, ==cos(tilt)
		if (w1 > w2) { // rotate tilt by PI/2
			ratio = w2/w1;
			beta += Math.PI/2; // start with rotation (last in matrices)
			if (beta > Math.PI) {
				beta -= 2* Math.PI;
			}
		}
		
		if (beta >= Math.PI/2) {
			beta -= Math.PI;
		} else if (beta < -Math.PI) {
			beta += Math.PI;
		}
		
		gamma = rot-beta;
		while (gamma >= Math.PI/2) {
			gamma -= Math.PI;
		}
		while (gamma < -Math.PI/2) {
			gamma += Math.PI;
		}
		
		if (y_down_ccw) {
			beta *= -1; 
			gamma *= -1; 
			rot *= -1; // pure rotation
		}
		this.y_down_ccw = y_down_ccw;
		st = true;
	}
	
	
}
