class Quat
{
	private double x = 0;
	private double y = 0;
	private double z = 0;
	private double w = 1;

	public Quat()
	{
	}

	public Quat(double roll, double pitch, double yaw)
	{
		double euler[] = { roll, pitch, yaw };
		this.set(euler);
	}

	public void set(double euler[])
	{
		// Calculate sin/cos of roll/pitch/yaw
		double sr = Math.sin(euler[0] * 0.5f);
		double cr = Math.cos(euler[0] * 0.5f);

		double sp = Math.sin(euler[1] * 0.5f);
		double cp = Math.cos(euler[1] * 0.5f);

		double sy = Math.sin(euler[2] * 0.5f);
		double cy = Math.cos(euler[2] * 0.5f);

		// Calculate quaternion
		double tx = (cr * cp * sy) + (sr * sp * cy);
		double ty = (sr * cp * cy) + (cr * sp * sy);
		double tz = (cr * sp * cy) - (sr * cp * sy);
		double tw = (cr * cp * cy) - (sr * sp * sy);

	  	// Normalize output
	  	double dist = Math.sqrt(tx*tx + ty*ty + tz*tz + tw*tw);

		this.x = tx / dist;
		this.y = ty / dist;
		this.z = tz / dist;
		this.w = tw / dist;
	}

	public void get(double[] euler)
	{
		double test = x*y + z*w;

		if (test > 0.499) {
			// north pole
			euler[0] = 2 * Math.atan2(x, w);
			euler[1] = Math.PI / 2;
			euler[2] = 0;
		} else if (test < -0.499) {
			// south pole
			euler[0] = -2 * Math.atan2(x, w);
			euler[1] = Math.PI / 2;
			euler[2] = 0;
		} else {
			// normal
			euler[0] = Math.atan2(2*y*w - 2*x*z, 1 - 2*(y*y) - 2*(z*z));
			euler[1] = Math.asin(2*x*y + 2*z*w);
			euler[2] = Math.atan2(2*x*w - 2*y*z, 1 - 2*(x*x) - 2*(z*z));
		}
	}

	public void mul(Quat q)
	{
		double tw = (w * q.w) - (x * q.x) - (y * q.y) - (z * q.z);
		double tx = (w * q.x) + (x * q.w) + (y * q.z) - (z * q.y);
		double ty = (w * q.y) - (x * q.z) + (y * q.w) + (z * q.x);
		double tz = (w * q.z) + (x * q.y) - (y * q.x) + (z * q.w);

		this.w = tw;
		this.x = tx;
		this.y = ty;
		this.z = tz;
	}

	public void print(String label)
	{
		double euler[] = {0, 0, 0};
		this.get(euler);
		System.out.format("%8s rpy - %7.2f %7.2f %7.2f\n",
				label + ":",
				euler[0]*180/Math.PI,
				euler[1]*180/Math.PI,
				euler[2]*180/Math.PI);
	}

	public static void test() {
		Quat id  = new Quat(0, 0, 0);
		Quat r90 = new Quat(Math.PI/2, 0, 0);
		Quat p90 = new Quat(0, Math.PI/2, 0);
		Quat y90 = new Quat(0, 0, Math.PI/2);

		Quat sum = new Quat();

		System.out.println("Init");
		r90.print("r90");
		p90.print("p90");
		y90.print("y90");

		System.out.println("Roll:");
		sum.mul(r90); sum.print("sum");
		sum.mul(r90); sum.print("sum");
		sum.mul(r90); sum.print("sum");
		sum.mul(r90); sum.print("sum");

		System.out.println("Pitch:");
		sum.mul(p90); sum.print("sum");
		sum.mul(p90); sum.print("sum");
		sum.mul(p90); sum.print("sum");
		sum.mul(p90); sum.print("sum");

		System.out.println("Yaw:");
		sum.mul(y90); sum.print("sum");
		sum.mul(y90); sum.print("sum");
		sum.mul(y90); sum.print("sum");
		sum.mul(y90); sum.print("sum");

		System.out.println("Test:");
		sum.mul(r90); sum.print("sum");
		sum.mul(p90); sum.print("sum");
		sum.mul(y90); sum.print("sum");
		sum.mul(p90); sum.print("sum");
		sum.mul(y90); sum.print("sum");
		sum.mul(y90); sum.print("sum");
	}
}