package mrdev023.math;

public class Quaternion {

	public float x,y,z,w;
	
	public Quaternion(){
		x = 0;
		y = 0;
		z = 0;
		w = 0;
	}
	
	public Quaternion(Vector3f axis,float angle){
		float sin = Mathf.sin(Mathf.toRadians(angle/2.0f));
		float cos = Mathf.cos(Mathf.toRadians(angle/2.0f));
		x = axis.getX() * sin;
		y = axis.getY() * sin;
		z = axis.getZ() * sin;
		w = cos;
	}
	
	public Quaternion(Vector3f rot){
		this(rot.x,rot.y,rot.z);
	}
	
	public Quaternion (float yaw, float roll, float pitch) {
		yaw = Mathf.toRadians(yaw);
		roll = Mathf.toRadians(roll);
		pitch = Mathf.toRadians(pitch);
        float angle;
        float sinRoll, sinPitch, sinYaw, cosRoll, cosPitch, cosYaw;
        angle = pitch * 0.5f;
        sinPitch = Mathf.sin(angle);
        cosPitch = Mathf.cos(angle);
        angle = roll * 0.5f;
        sinRoll = Mathf.sin(angle);
        cosRoll = Mathf.cos(angle);
        angle = yaw * 0.5f;
        sinYaw = Mathf.sin(angle);
        cosYaw = Mathf.cos(angle);

        // variables used to reduce multiplication calls.
        float cosRollXcosPitch = cosRoll * cosPitch;
        float sinRollXsinPitch = sinRoll * sinPitch;
        float cosRollXsinPitch = cosRoll * sinPitch;
        float sinRollXcosPitch = sinRoll * cosPitch;
        
        w = (cosRollXcosPitch * cosYaw - sinRollXsinPitch * sinYaw);
        x = (cosRollXcosPitch * sinYaw + sinRollXsinPitch * cosYaw);
        y = (sinRollXcosPitch * cosYaw + cosRollXsinPitch * sinYaw);
        z = (cosRollXsinPitch * cosYaw - sinRollXcosPitch * sinYaw);
        
        normalize();
    }
	
	public void normalize(){
		float n = (float)(1.0/Math.sqrt(norm()));
        x *= n;
        y *= n;
        z *= n;
        w *= n;
	}
	
	public float norm(){
		return w * w + x * x + y * y + z * z;
	}
	
	public Quaternion Euler(Vector3f rot) {
	    x = Mathf.toRadians(rot.x);
	    y = Mathf.toRadians(rot.y);
	    z = Mathf.toRadians(rot.z);
		float c1 = Mathf.cos(y/2);
		float s1 = Mathf.sin(y/2);
		float c2 = Mathf.cos(z/2);
		float s2 = Mathf.sin(z/2);
		float c3 = Mathf.cos(x/2);
		float s3 = Mathf.sin(x/2);
	    float c1c2 = c1*c2;
	    float s1s2 = s1*s2;
	    this.w =c1c2*c3 - s1s2*s3;
	  	this.x =c1c2*s3 + s1s2*c3;
		this.y =s1*c2*c3 + c1*s2*s3;
		this.z =c1*s2*c3 - s1*c2*s3;
		return new Quaternion(x, y, z, w);
	}
	
	public Vector3f toEulerAngles(){
		Vector3f euler = new Vector3f();

	    float sqw = w * w;
	    float sqx = x * x;
	    float sqy = y * y;
	    float sqz = z * z;
	    float unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise
	                                                                            // is correction factor
	    float test = x * y + z * w;
	    if (test > 0.499 * unit) { // singularity at north pole
	            euler.y = 2 * Mathf.atan2(x, w);
	            euler.z = Mathf.PI/2.0f;
	            euler.x = 0;
	    } else if (test < -0.499 * unit) { // singularity at south pole
	    		euler.y = -2 * Mathf.atan2(x, w);
	    		euler.z = -Mathf.PI/2.0f;
	    		euler.x = 0;
	    } else {
	    		euler.y = Mathf.atan2(2 * y * w - 2 * x * z, sqx - sqy - sqz + sqw); // roll or heading 
	    		euler.z = Mathf.asin(2 * test / unit); // pitch or attitude
	    		euler.x = Mathf.atan2(2 * x * w - 2 * y * z, -sqx + sqy - sqz + sqw); // yaw or bank
	    }
        return euler.toDegrees();
	}
	
	public Quaternion(float axisX,float axisY,float axisZ,float angle){
		float sin = Mathf.sin(Mathf.toRadians(angle/2.0f));
		float cos = Mathf.cos(Mathf.toRadians(angle/2.0f));
		x = axisX * sin;
		y = axisY * sin;
		z = axisZ * sin;
		w = cos;
	}
	
	public Matrix4f toMatrixRotation(){
		Vector3f forward =  new Vector3f(2.0f * (x * z - w * y), 2.0f * (y * z + w * x), 1.0f - 2.0f * (x * x + y * y));
		Vector3f up = new Vector3f(2.0f * (x * y + w * z), 1.0f - 2.0f * (x * x + z * z), 2.0f * (y * z - w * x));
		Vector3f right = new Vector3f(1.0f - 2.0f * (y * y + z * z), 2.0f * (x * y - w * z), 2.0f * (x * z + w * y));

		return Matrix4f.rotate(forward, up, right);
	}
	
}