package edu.ucla.iBeaconNav;

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

	public Quat()
	{
	}

	public Quat(double roll, double pitch, double yaw)
	{
		this.set(new Vect(roll, pitch, yaw));
	}

	public void set(Vect rpy)
	{
		// Calculate sin/cos of roll/pitch/yaw
		double sr = Math.sin(rpy.x * 0.5f);
		double cr = Math.cos(rpy.x * 0.5f);

		double sp = Math.sin(rpy.y * 0.5f);
		double cp = Math.cos(rpy.y * 0.5f);

		double sy = Math.sin(rpy.z * 0.5f);
		double cy = Math.cos(rpy.z * 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(Vect rpy)
	{
		double test = x*y + z*w;

		if (test > 0.499) {
			// north pole
			rpy.x = 2 * Math.atan2(x, w);
			rpy.y = Math.PI / 2;
			rpy.z = 0;
		} else if (test < -0.499) {
			// south pole
			rpy.x = -2 * Math.atan2(x, w);
			rpy.y = Math.PI / 2;
			rpy.z = 0;
		} else {
			// normal
			rpy.x = Math.atan2(2*y*w - 2*x*z, 1 - 2*(y*y) - 2*(z*z));
			rpy.y = Math.asin(2*x*y + 2*z*w);
			rpy.z = 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 String toString()
	{
		Vect rpy = new Vect();
		this.get(rpy);
		return String.format("%7.2f %7.2f %7.2f\n",
				rpy.x*180/Math.PI,
				rpy.y*180/Math.PI,
				rpy.z*180/Math.PI);
	}

	public void print(String label)
	{
		System.out.format("%-8s rpy - %s", label+":", this);
	}

	public void debug(String label)
	{
		Util.debug(String.format("%-8s rpy - %s", label+":", this));
	}

	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");
	}
}