/*
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The ASF licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package org.apache.commons.math.geometry;

import org.apache.commons.math.geometry.CardanEulerSingularityException;
import org.apache.commons.math.geometry.NotARotationMatrixException;
import org.apache.commons.math.geometry.Rotation;
import org.apache.commons.math.geometry.RotationOrder;
import org.apache.commons.math.geometry.Vector3D;
import org.apache.commons.math.util.MathUtils;

import junit.framework.*;

public class RotationTest extends TestCase {

	public RotationTest(String name) {
		super(name);
	}

	public void testIdentity() {

		Rotation r = Rotation.IDENTITY;
		checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I);
		checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J);
		checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K);
		checkAngle(r.getAngle(), 0);

		r = new Rotation(-1, 0, 0, 0, false);
		checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I);
		checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J);
		checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K);
		checkAngle(r.getAngle(), 0);

		r = new Rotation(42, 0, 0, 0, true);
		checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I);
		checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J);
		checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K);
		checkAngle(r.getAngle(), 0);

	}

	public void testAxisAngle() {

		Rotation r = new Rotation(new Vector3D(10, 10, 10), 2 * Math.PI / 3);
		checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_J);
		checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_K);
		checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_I);
		double s = 1 / Math.sqrt(3);
		checkVector(r.getAxis(), new Vector3D(s, s, s));
		checkAngle(r.getAngle(), 2 * Math.PI / 3);

		try {
			new Rotation(new Vector3D(0, 0, 0), 2 * Math.PI / 3);
			fail("an exception should have been thrown");
		} catch (ArithmeticException e) {
		} catch (Exception e) {
			fail("unexpected exception");
		}

		r = new Rotation(Vector3D.PLUS_K, 1.5 * Math.PI);
		checkVector(r.getAxis(), new Vector3D(0, 0, -1));
		checkAngle(r.getAngle(), 0.5 * Math.PI);

		r = new Rotation(Vector3D.PLUS_J, Math.PI);
		checkVector(r.getAxis(), Vector3D.PLUS_J);
		checkAngle(r.getAngle(), Math.PI);

		checkVector(Rotation.IDENTITY.getAxis(), Vector3D.PLUS_I);

	}

	public void testRevert() {
		Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
		Rotation reverted = r.revert();
		checkRotation(r.applyTo(reverted), 1, 0, 0, 0);
		checkRotation(reverted.applyTo(r), 1, 0, 0, 0);
		assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12);
		assertEquals(-1, Vector3D.dotProduct(r.getAxis(), reverted.getAxis()), 1.0e-12);
	}

	public void testVectorOnePair() {

		Vector3D u = new Vector3D(3, 2, 1);
		Vector3D v = new Vector3D(-4, 2, 2);
		Rotation r = new Rotation(u, v);
		checkVector(r.applyTo(u.scalarMultiply(v.getNorm())), v.scalarMultiply(u.getNorm()));

		checkAngle(new Rotation(u, u.negate()).getAngle(), Math.PI);

		try {
			new Rotation(u, Vector3D.ZERO);
			fail("an exception should have been thrown");
		} catch (IllegalArgumentException e) {
			// expected behavior
		} catch (Exception e) {
			fail("unexpected exception");
		}

	}

	public void testVectorTwoPairs() {

		Vector3D u1 = new Vector3D(3, 0, 0);
		Vector3D u2 = new Vector3D(0, 5, 0);
		Vector3D v1 = new Vector3D(0, 0, 2);
		Vector3D v2 = new Vector3D(-2, 0, 2);
		Rotation r = new Rotation(u1, u2, v1, v2);
		checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_K);
		checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.MINUS_I);

		r = new Rotation(u1, u2, u1.negate(), u2.negate());
		Vector3D axis = r.getAxis();
		if (Vector3D.dotProduct(axis, Vector3D.PLUS_K) > 0) {
			checkVector(axis, Vector3D.PLUS_K);
		} else {
			checkVector(axis, Vector3D.MINUS_K);
		}
		checkAngle(r.getAngle(), Math.PI);

		double sqrt = Math.sqrt(2) / 2;
		r = new Rotation(Vector3D.PLUS_I, Vector3D.PLUS_J, new Vector3D(0.5, 0.5, sqrt), new Vector3D(0.5, 0.5, -sqrt));
		checkRotation(r, sqrt, 0.5, 0.5, 0);

		r = new Rotation(u1, u2, u1, Vector3D.crossProduct(u1, u2));
		checkRotation(r, sqrt, -sqrt, 0, 0);

		checkRotation(new Rotation(u1, u2, u1, u2), 1, 0, 0, 0);

		try {
			new Rotation(u1, u2, Vector3D.ZERO, v2);
			fail("an exception should have been thrown");
		} catch (IllegalArgumentException e) {
			// expected behavior
		} catch (Exception e) {
			fail("unexpected exception");
		}

	}

	public void testMatrix() throws NotARotationMatrixException {

		try {
			new Rotation(new double[][] { { 0.0, 1.0, 0.0 }, { 1.0, 0.0, 0.0 } }, 1.0e-7);
		} catch (NotARotationMatrixException nrme) {
			// expected behavior
		} catch (Exception e) {
			fail("wrong exception caught: " + e.getMessage());
		}

		try {
			new Rotation(new double[][] { { 0.445888, 0.797184, -0.407040 }, { 0.821760, -0.184320, 0.539200 },
					{ -0.354816, 0.574912, 0.737280 } }, 1.0e-7);
		} catch (NotARotationMatrixException nrme) {
			// expected behavior
		} catch (Exception e) {
			fail("wrong exception caught: " + e.getMessage());
		}

		try {
			new Rotation(new double[][] { { 0.4, 0.8, -0.4 }, { -0.4, 0.6, 0.7 }, { 0.8, -0.2, 0.5 } }, 1.0e-15);
		} catch (NotARotationMatrixException nrme) {
			// expected behavior
		} catch (Exception e) {
			fail("wrong exception caught: " + e.getMessage());
		}

		checkRotation(new Rotation(new double[][] { { 0.445888, 0.797184, -0.407040 },
				{ -0.354816, 0.574912, 0.737280 }, { 0.821760, -0.184320, 0.539200 } }, 1.0e-10), 0.8, 0.288, 0.384,
				0.36);

		checkRotation(new Rotation(new double[][] { { 0.539200, 0.737280, 0.407040 }, { 0.184320, -0.574912, 0.797184 },
				{ 0.821760, -0.354816, -0.445888 } }, 1.0e-10), 0.36, 0.8, 0.288, 0.384);

		checkRotation(new Rotation(new double[][] { { -0.445888, 0.797184, -0.407040 },
				{ 0.354816, 0.574912, 0.737280 }, { 0.821760, 0.184320, -0.539200 } }, 1.0e-10), 0.384, 0.36, 0.8,
				0.288);

		checkRotation(new Rotation(new double[][] { { -0.539200, 0.737280, 0.407040 },
				{ -0.184320, -0.574912, 0.797184 }, { 0.821760, 0.354816, 0.445888 } }, 1.0e-10), 0.288, 0.384, 0.36,
				0.8);

		double[][] m1 = { { 0.0, 1.0, 0.0 }, { 0.0, 0.0, 1.0 }, { 1.0, 0.0, 0.0 } };
		Rotation r = new Rotation(m1, 1.0e-7);
		checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_K);
		checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_I);
		checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_J);

		double[][] m2 = { { 0.83203, -0.55012, -0.07139 }, { 0.48293, 0.78164, -0.39474 },
				{ 0.27296, 0.29396, 0.91602 } };
		r = new Rotation(m2, 1.0e-12);

		double[][] m3 = r.getMatrix();
		double d00 = m2[0][0] - m3[0][0];
		double d01 = m2[0][1] - m3[0][1];
		double d02 = m2[0][2] - m3[0][2];
		double d10 = m2[1][0] - m3[1][0];
		double d11 = m2[1][1] - m3[1][1];
		double d12 = m2[1][2] - m3[1][2];
		double d20 = m2[2][0] - m3[2][0];
		double d21 = m2[2][1] - m3[2][1];
		double d22 = m2[2][2] - m3[2][2];

		assertTrue(Math.abs(d00) < 6.0e-6);
		assertTrue(Math.abs(d01) < 6.0e-6);
		assertTrue(Math.abs(d02) < 6.0e-6);
		assertTrue(Math.abs(d10) < 6.0e-6);
		assertTrue(Math.abs(d11) < 6.0e-6);
		assertTrue(Math.abs(d12) < 6.0e-6);
		assertTrue(Math.abs(d20) < 6.0e-6);
		assertTrue(Math.abs(d21) < 6.0e-6);
		assertTrue(Math.abs(d22) < 6.0e-6);

		assertTrue(Math.abs(d00) > 4.0e-7);
		assertTrue(Math.abs(d01) > 4.0e-7);
		assertTrue(Math.abs(d02) > 4.0e-7);
		assertTrue(Math.abs(d10) > 4.0e-7);
		assertTrue(Math.abs(d11) > 4.0e-7);
		assertTrue(Math.abs(d12) > 4.0e-7);
		assertTrue(Math.abs(d20) > 4.0e-7);
		assertTrue(Math.abs(d21) > 4.0e-7);
		assertTrue(Math.abs(d22) > 4.0e-7);

		for (int i = 0; i < 3; ++i) {
			for (int j = 0; j < 3; ++j) {
				double m3tm3 = m3[i][0] * m3[j][0] + m3[i][1] * m3[j][1] + m3[i][2] * m3[j][2];
				if (i == j) {
					assertTrue(Math.abs(m3tm3 - 1.0) < 1.0e-10);
				} else {
					assertTrue(Math.abs(m3tm3) < 1.0e-10);
				}
			}
		}

		checkVector(r.applyTo(Vector3D.PLUS_I), new Vector3D(m3[0][0], m3[1][0], m3[2][0]));
		checkVector(r.applyTo(Vector3D.PLUS_J), new Vector3D(m3[0][1], m3[1][1], m3[2][1]));
		checkVector(r.applyTo(Vector3D.PLUS_K), new Vector3D(m3[0][2], m3[1][2], m3[2][2]));

		double[][] m4 = { { 1.0, 0.0, 0.0 }, { 0.0, -1.0, 0.0 }, { 0.0, 0.0, -1.0 } };
		r = new Rotation(m4, 1.0e-7);
		checkAngle(r.getAngle(), Math.PI);

		try {
			double[][] m5 = { { 0.0, 0.0, 1.0 }, { 0.0, 1.0, 0.0 }, { 1.0, 0.0, 0.0 } };
			r = new Rotation(m5, 1.0e-7);
			fail("got " + r + ", should have caught an exception");
		} catch (NotARotationMatrixException e) {
			// expected
		} catch (Exception e) {
			fail("wrong exception caught");
		}

	}

	public void testAngles() throws CardanEulerSingularityException {

		RotationOrder[] CardanOrders = { RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ, RotationOrder.YZX,
				RotationOrder.ZXY, RotationOrder.ZYX };

		for (int i = 0; i < CardanOrders.length; ++i) {
			for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
				for (double alpha2 = -1.55; alpha2 < 1.55; alpha2 += 0.3) {
					for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
						Rotation r = new Rotation(CardanOrders[i], alpha1, alpha2, alpha3);
						double[] angles = r.getAngles(CardanOrders[i]);
						checkAngle(angles[0], alpha1);
						checkAngle(angles[1], alpha2);
						checkAngle(angles[2], alpha3);
					}
				}
			}
		}

		RotationOrder[] EulerOrders = { RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY, RotationOrder.YZY,
				RotationOrder.ZXZ, RotationOrder.ZYZ };

		for (int i = 0; i < EulerOrders.length; ++i) {
			for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
				for (double alpha2 = 0.05; alpha2 < 3.1; alpha2 += 0.3) {
					for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
						Rotation r = new Rotation(EulerOrders[i], alpha1, alpha2, alpha3);
						double[] angles = r.getAngles(EulerOrders[i]);
						checkAngle(angles[0], alpha1);
						checkAngle(angles[1], alpha2);
						checkAngle(angles[2], alpha3);
					}
				}
			}
		}

	}

	public void testSingularities() {

		RotationOrder[] CardanOrders = { RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ, RotationOrder.YZX,
				RotationOrder.ZXY, RotationOrder.ZYX };

		double[] singularCardanAngle = { Math.PI / 2, -Math.PI / 2 };
		for (int i = 0; i < CardanOrders.length; ++i) {
			for (int j = 0; j < singularCardanAngle.length; ++j) {
				Rotation r = new Rotation(CardanOrders[i], 0.1, singularCardanAngle[j], 0.3);
				try {
					r.getAngles(CardanOrders[i]);
					fail("an exception should have been caught");
				} catch (CardanEulerSingularityException cese) {
					// expected behavior
				} catch (Exception e) {
					fail("wrong exception caught: " + e.getMessage());
				}
			}
		}

		RotationOrder[] EulerOrders = { RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY, RotationOrder.YZY,
				RotationOrder.ZXZ, RotationOrder.ZYZ };

		double[] singularEulerAngle = { 0, Math.PI };
		for (int i = 0; i < EulerOrders.length; ++i) {
			for (int j = 0; j < singularEulerAngle.length; ++j) {
				Rotation r = new Rotation(EulerOrders[i], 0.1, singularEulerAngle[j], 0.3);
				try {
					r.getAngles(EulerOrders[i]);
					fail("an exception should have been caught");
				} catch (CardanEulerSingularityException cese) {
					// expected behavior
				} catch (Exception e) {
					fail("wrong exception caught: " + e.getMessage());
				}
			}
		}

	}

	public void testQuaternion() {

		Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7);
		double n = 23.5;
		Rotation r2 = new Rotation(n * r1.getQ0(), n * r1.getQ1(), n * r1.getQ2(), n * r1.getQ3(), true);
		for (double x = -0.9; x < 0.9; x += 0.2) {
			for (double y = -0.9; y < 0.9; y += 0.2) {
				for (double z = -0.9; z < 0.9; z += 0.2) {
					Vector3D u = new Vector3D(x, y, z);
					checkVector(r2.applyTo(u), r1.applyTo(u));
				}
			}
		}

		r1 = new Rotation(0.288, 0.384, 0.36, 0.8, false);
		checkRotation(r1, -r1.getQ0(), -r1.getQ1(), -r1.getQ2(), -r1.getQ3());

	}

	public void testCompose() {

		Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7);
		Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3);
		Rotation r3 = r2.applyTo(r1);

		for (double x = -0.9; x < 0.9; x += 0.2) {
			for (double y = -0.9; y < 0.9; y += 0.2) {
				for (double z = -0.9; z < 0.9; z += 0.2) {
					Vector3D u = new Vector3D(x, y, z);
					checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
				}
			}
		}

	}

	public void testComposeInverse() {

		Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7);
		Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3);
		Rotation r3 = r2.applyInverseTo(r1);

		for (double x = -0.9; x < 0.9; x += 0.2) {
			for (double y = -0.9; y < 0.9; y += 0.2) {
				for (double z = -0.9; z < 0.9; z += 0.2) {
					Vector3D u = new Vector3D(x, y, z);
					checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u));
				}
			}
		}

	}

	public void testApplyInverseTo() {

		Rotation r = new Rotation(new Vector3D(2, -3, 5), 1.7);
		for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
			for (double phi = -1.55; phi < 1.55; phi += 0.2) {
				Vector3D u = new Vector3D(Math.cos(lambda) * Math.cos(phi), Math.sin(lambda) * Math.cos(phi),
						Math.sin(phi));
				r.applyInverseTo(r.applyTo(u));
				checkVector(u, r.applyInverseTo(r.applyTo(u)));
				checkVector(u, r.applyTo(r.applyInverseTo(u)));
			}
		}

		r = Rotation.IDENTITY;
		for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
			for (double phi = -1.55; phi < 1.55; phi += 0.2) {
				Vector3D u = new Vector3D(Math.cos(lambda) * Math.cos(phi), Math.sin(lambda) * Math.cos(phi),
						Math.sin(phi));
				checkVector(u, r.applyInverseTo(r.applyTo(u)));
				checkVector(u, r.applyTo(r.applyInverseTo(u)));
			}
		}

		r = new Rotation(Vector3D.PLUS_K, Math.PI);
		for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
			for (double phi = -1.55; phi < 1.55; phi += 0.2) {
				Vector3D u = new Vector3D(Math.cos(lambda) * Math.cos(phi), Math.sin(lambda) * Math.cos(phi),
						Math.sin(phi));
				checkVector(u, r.applyInverseTo(r.applyTo(u)));
				checkVector(u, r.applyTo(r.applyInverseTo(u)));
			}
		}

	}

	private void checkVector(Vector3D v1, Vector3D v2) {
		assertTrue(v1.subtract(v2).getNorm() < 1.0e-10);
	}

	private void checkAngle(double a1, double a2) {
		assertEquals(a1, MathUtils.normalizeAngle(a2, a1), 1.0e-10);
	}

	private void checkRotation(Rotation r, double q0, double q1, double q2, double q3) {
		assertEquals(0, Rotation.distance(r, new Rotation(q0, q1, q2, q3, false)), 1.0e-12);
	}

	public static Test suite() {
		return new TestSuite(RotationTest.class);
	}

}
