/*******************************************************************************
 * Copyright (c) 2012, 2013 Association for Decentralized Information Management in
 * Industry THTH ry.
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the Eclipse Public License v1.0
 * which accompanies this distribution, and is available at
 * http://www.eclipse.org/legal/epl-v10.html
 *
 * Contributors:
 *     VTT Technical Research Centre of Finland - initial API and implementation
 *******************************************************************************/
package org.simantics.g3d.math;

import javax.vecmath.AxisAngle4d;
import javax.vecmath.Quat4d;
import javax.vecmath.Vector3d;

public class EulerTools {

	public enum Order {
		XYX, XYZ, XZX, XZY, YXY, YXZ, YZX, YZY, ZXY, ZXZ, ZYX, ZYZ
	};

	public static Quat4d getQuatFromEuler(Order order, Vector3d a) {
		return getQuatFromEuler(order, a.x, a.y, a.z);
	}
	
	public static Quat4d getQuatFromEuler(Order order, double a1, double a2, double a3) {
		Quat4d q1 = new Quat4d();
		Quat4d q2 = new Quat4d();
		Quat4d q3 = new Quat4d();
		switch (order) {
		case XYX:
			q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));
			q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));
			q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));
			break;
		case XYZ:
			q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));
			q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));
			q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));
			break;
		case XZX:
			q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));
			q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));
			q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));
			break;
		case XZY:
			q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));
			q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));
			q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));
			break;
		case YXY:
			q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));
			q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));
			q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));
			break;
		case YXZ:
			q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));
			q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));
			q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));
			break;
		case YZX:
			q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));
			q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));
			q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));
			break;
		case YZY:
			q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));
			q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));
			q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));
			break;
		case ZXY:
			q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));
			q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));
			q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));
			break;
		case ZXZ:
			q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));
			q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));
			q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));
			break;
		case ZYX:
			q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));
			q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));
			q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));
			break;
		case ZYZ:
			q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));
			q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));
			q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));
			break;
		}
		q1.mul(q2);
		q1.mul(q3);
		return q1;
	}
	
	/**
	 * See http://noelhughes.net/uploads/quat_2_euler_paper_ver3.pdf
	 * @param order
	 * @param q
	 * @return
	 */
	public static Vector3d getEulerFromQuat(Order order, Quat4d q) {
		Vector3d euler = new Vector3d();
		
//		Vector3d v1 = new Vector3d();
//		Vector3d v2 = new Vector3d();
		Vector3d v3 = new Vector3d();
		Vector3d v3n = new Vector3d();
		
		switch (order) {
		case XYX:
//			v1.x = 1.0;
//			v2.y = 1.0;
			v3.x = 1.0;
			v3n.y = 1.0;
			break;
		case XYZ:
//			v1.x = 1.0;
//			v2.y = 1.0;
			v3.z = 1.0;
			v3n.x = 1.0;
			break;
		case XZX:
//			v1.x = 1.0;
//			v2.z = 1.0;
			v3.x = 1.0;
			v3n.y = 1.0;
			break;
		case XZY:
//			v1.x = 1.0;
//			v2.z = 1.0;
			v3.y = 1.0;
			v3n.z = 1.0;
			break;
		case YXY:
//			v1.y = 1.0;
//			v2.x = 1.0;
			v3.y = 1.0;
			v3n.z = 1.0;
			break;
		case YXZ:
//			v1.y = 1.0;
//			v2.x = 1.0;
			v3.z = 1.0;
			v3n.x = 1.0;
			break;
		case YZX:
//			v1.y = 1.0;
//			v2.z = 1.0;
			v3.x = 1.0;
			v3n.y = 1.0;
			break;
		case YZY:
//			v1.y = 1.0;
//			v2.z = 1.0;
			v3.y = 1.0;
			v3n.z = 1.0;
			break;
		case ZXY:
//			v1.z = 1.0;
//			v2.x = 1.0;
			v3.y = 1.0;
			v3n.z = 1.0;
			break;
		case ZXZ:
//			v1.z = 1.0;
//			v2.x = 1.0;
			v3.z = 1.0;
			v3n.x = 1.0;
			break;
		case ZYX:
//			v1.z = 1.0;
//			v2.y = 1.0;
			v3.x = 1.0;
			v3n.y = 1.0;
			break;
		case ZYZ:
//			v1.z = 1.0;
//			v2.y = 1.0;
			v3.z = 1.0;
			v3n.x = 1.0;
			break;
		}
		Vector3d v3r = new Vector3d();
		MathTools.rotate(q, v3, v3r);
		v3r.normalize();
		
		switch (order) {

		case XZX:
			euler.x = Math.atan2(v3r.z, v3r.y);
			euler.y = Math.acos(v3r.x);
			break;
		case YXY:
			euler.x = Math.atan2(v3r.x, v3r.z);
			euler.y = Math.acos(v3r.y);
			break;
		case ZYZ:
			euler.x = Math.atan2(v3r.y, v3r.x);
			euler.y = Math.acos(v3r.z);
			break;
			
		case XZY:
			euler.x = Math.atan2(v3r.z, v3r.y);
			euler.y = -Math.asin(v3r.x);
			break;
		case YXZ:
			euler.x = Math.atan2(v3r.x, v3r.z);
			euler.y = -Math.asin(v3r.y);
			break;
		case ZYX:
			euler.x = Math.atan2(v3r.y, v3r.x);
			euler.y = -Math.asin(v3r.z);
			break;
			
		case XYX:
			euler.x = Math.atan2(v3r.y, -v3r.z);
			//euler.x = Math.atan2(v3r.y, -v3r.x);
			euler.y = Math.acos(v3r.x);
			break;
		case YZY:
			euler.x = Math.atan2(v3r.z, -v3r.x);
			//euler.x = Math.atan2(v3r.z, -v3r.y);
			euler.y = Math.acos(v3r.y);
			break;
		case ZXZ:
			euler.x = Math.atan2(v3r.x, -v3r.y);
			//euler.x = Math.atan2(v3r.x, -v3r.z);
			euler.y = Math.acos(v3r.z);
			break;
			
		case XYZ:
			euler.x = Math.atan2(-v3r.y, v3r.z);
			euler.y = Math.asin(v3r.x);
			break;
		case YZX:
			euler.x = Math.atan2(-v3r.z, v3r.x);
			euler.y = Math.asin(v3r.y);
			break;
		case ZXY:
			euler.x = Math.atan2(-v3r.x, v3r.y);
			euler.y = Math.asin(v3r.z);
			break;
		}
		
		Quat4d q1 = new Quat4d();
		q1.w = Math.cos(euler.x*0.5);
		Quat4d q2 = new Quat4d();
		q2.w = Math.cos(euler.y*0.5);
		
		switch (order) {
		case XYX:
		case XYZ:
		case XZX:
		case XZY:
			q1.x = Math.sin(euler.x*0.5);
			break;
		case YXY:
		case YXZ:
		case YZX:
		case YZY:
			q1.y = Math.sin(euler.x*0.5);
			break;
		case ZXY:
		case ZXZ:
		case ZYX:
		case ZYZ:
			q1.z = Math.sin(euler.x*0.5);
			break;
		}
		
		switch (order) {
		case YXY:
		case YXZ:
		case ZXY:
		case ZXZ:
			q2.x = Math.sin(euler.y*0.5);
			break;
		case XYX:
		case XYZ:
		case ZYX:
		case ZYZ:
			q2.y = Math.sin(euler.y*0.5);
			break;
		case XZX:
		case XZY:
		case YZX:
		case YZY:
			q2.z = Math.sin(euler.y*0.5);
			break;
		}
		
		Quat4d q12 = new Quat4d();
		q12.mul(q1, q2);
		
		Vector3d v3n12 = new Vector3d();
		Vector3d v3ng = new Vector3d();
		MathTools.rotate(q12, v3n, v3n12);
		MathTools.rotate(q, v3n, v3ng);
		
		double dot = v3n12.dot(v3ng);
		dot = MathTools.clamp(-1.0, 1.0, dot);
		euler.z = Math.abs(Math.acos(dot));
		Vector3d vc = new Vector3d();
		vc.cross(v3n12, v3ng);
		euler.z *= Math.signum(vc.dot(v3r));
		
		return euler;
	}

	
	public static void main(String args[]) {
		
		boolean all = false;
		boolean allOrder = false;
		if (all) {
			testAll();
		} else if (allOrder) {
			test(Order.YXZ);
		} else {
			//test(Order.ZXY,30,60,45);
			//test(Order.YXZ,30,0,0);
			//test(Order.YXZ,30,90,60);
			test(Order.YXZ,300,240,360);
		}
	}
	
	private static void testAll() {
		double start = 0.0;
		double end = 90.0;
		double step = 30.0;
		for (double a1 = start;  a1 <= end; a1+= step) {
			double r1 = MathTools.degToRad(a1);
			for (double a2 = start;  a2 <= end; a2+= step) {
				double r2 = MathTools.degToRad(a2);
				for (double a3 = start;  a3 <= end; a3+= step) {
					double r3 = MathTools.degToRad(a3);
					for (Order order : Order.values()) {
						Quat4d q = EulerTools.getQuatFromEuler(order, r1, r2, r3);
						Vector3d a = EulerTools.getEulerFromQuat(order, q);
						Quat4d q2 = EulerTools.getQuatFromEuler(order, a.x,a.y,a.z);
						a.x = MathTools.radToDeg(a.x);
						a.y = MathTools.radToDeg(a.y);
						a.z = MathTools.radToDeg(a.z);
						System.out.println(toString(a1) +" " + toString(a2) + " " + toString(a3) + " " + order + "\t" + toString(a) + "\t" + toString(q) + "\t" + toString(q2));
					}
				}	
			}	
		}
	}
	
	private static void test(Order order) {
		double start = 0.0;
		double end = 360.0;
		double step = 30.0;
		for (double a1 = start;  a1 <= end; a1+= step) {
			double r1 = MathTools.degToRad(a1);
			for (double a2 = start;  a2 <= end; a2+= step) {
				double r2 = MathTools.degToRad(a2);
				for (double a3 = start;  a3 <= end; a3+= step) {
					double r3 = MathTools.degToRad(a3);
					
					Quat4d q = EulerTools.getQuatFromEuler(order, r1, r2, r3);
					Vector3d a = EulerTools.getEulerFromQuat(order, q);
					Quat4d q2 = EulerTools.getQuatFromEuler(order, a.x,a.y,a.z);
					a.x = MathTools.radToDeg(a.x);
					a.y = MathTools.radToDeg(a.y);
					a.z = MathTools.radToDeg(a.z);
					
					System.out.println(toString(a1) +" " + toString(a2) + " " + toString(a3) + " " + order + "\t" + toString(a) + "\t" + toString(q) + "\t" + toString(q2));
				}	
			}	
		}
	}
	
	private static String toString(double d) {
		return String.format("%1$6.2f", d);
	}
	
	private static String toString(Vector3d v) {
		return "("+toString(v.x) +", "+ toString(v.y) + ", " + toString(v.z) +")";
	}
	
	private static String toString(Quat4d v) {
		return "("+toString(v.x) +", "+ toString(v.y) + ", " + toString(v.z) + ", " + toString(v.w) +")";
	}
	
	private static void test(Order order, double deg1, double deg2, double deg3) {
		double r1 = MathTools.degToRad(deg1);
		double r2 = MathTools.degToRad(deg2);
		double r3 = MathTools.degToRad(deg3);
		
		Quat4d q = EulerTools.getQuatFromEuler(order, r1, r2, r3);
		Vector3d a = EulerTools.getEulerFromQuat(order, q);
		Quat4d q2 = EulerTools.getQuatFromEuler(order, a.x,a.y,a.z);
		a.x = MathTools.radToDeg(a.x);
		a.y = MathTools.radToDeg(a.y);
		a.z = MathTools.radToDeg(a.z);
		System.out.println(toString(deg1) +" " + toString(deg2) + " " + toString(deg3) + " " + order + "\t" + toString(a) + "\t" + toString(q) + "\t" + toString(q2));

	}
}