package com.elphel.imagej.orthomosaic;

public class SingularValueDecomposition {
	double beta;
	double gamma;
	double w1;
	double w2;
	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 double [][] getR1(){	return rotMatrix(beta);}
	public double [][] getR2(){	return rotMatrix(gamma);}
	public double [][] getIR1(){return rotMatrix(-beta);}
	public double [][] getRot(){return rotMatrix(rot);}
	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);}
	
	public String toString(boolean degrees) {
//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 fmt_rad = "scale=%10.8f,tilt= %10.7f, gamma=%10.7f, beta=%10.7f, rot=%10.7f, w1=%10.8f, w2=%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, ratio=%10.8f";
		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);
	}
	
	public double [][] getW(){
		return new double [][] {{w1,0},{0,w1}};
	}
	
	/**
	 * Decomposing linear transform into rotation
	 * 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;
		return svd;
	}
	/**
	 * 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);
		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;
	}
	
	
}
