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

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;
		return 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);
	}
	
	/**
	 * 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;
	}
	
	public static double [] multiplyScaled(
			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(invert(r), s);
	}	

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

	public static double [] divideScaled1(
			double [] r,
			double [] s	) {
		return multiply(invertScaled(r), s);
	}	

 */
	
	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 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 [] invert(
			double [] quat) {
		return new double [] {quat[0], -quat[1], -quat[2], -quat[3]};
	}

	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;
		}
		double [] axis = {ty/t,-tx/t,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;
	}
	
	/**
	 * 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;
	}
	
	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]
		 */
		double scale = normQuat(quat);
		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[1]*quat[1];
		double q33 = quat[1]*quat[1];
		double q03 = quat[0]*quat[3];
		double q12 = quat[1]*quat[2];
		
		double [][] affine = {
				{scale*(q00 + q11 - q22 - q33), scale*2*(q12 - q03)},
				{scale_y*2*(q03 + q12),         scale_y*(q00 - q11 + q22 - q33)}};
		if (invert) {
			affine = matInverse2x2(affine);
		}
		return affine;
	}
	/**
	 * Restore quaternion (rotation+scale) from affine traqnsform (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,
			boolean     y_down_ccw) {
		SingularValueDecomposition svd= SingularValueDecomposition.singularValueDecomposeScaleTiltBeta(
				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 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 [] 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 double [][] matInverse2x2(
			double [][] a) {
//			throw new IllegalArgumentException("Only [2][2] arrays");
		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]}};
	}
	
	
	
}
