package alternativa.math {
	import flash.geom.Vector3D;
	
	public class Quaternion {

		public var w:Number;
		public var x:Number;
		public var y:Number;
		public var z:Number;

		public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void {
			result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
			result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y;
			result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z;
			result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x;
		}

		public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion {
			var q:Quaternion = new Quaternion();
			q.setFromAxisAngle(axis, angle);
			return q;
		}

		public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion {
			var q:Quaternion = new Quaternion();
			q.setFromAxisAngleComponents(x, y, z, angle);
			return q;
		}
		
		private static var _q:Quaternion = new Quaternion();
		
		public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) {
			this.w = w;
			this.x = x;
			this.y = y;
			this.z = z;
		}

		public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion {
			this.w = w;
			this.x = x;
			this.y = y;
			this.z = z;
			return this;
		}

		public function normalize():Quaternion {
			var d:Number = w*w + x*x + y*y + z*z;
			if (d == 0) {
				w = 1;
			} else {
				d = 1/Math.sqrt(d);
				w *= d;
				x *= d;
				y *= d;
				z *= d;
			}
			return this;
		}

		/**
		 * Умножает на указанный кватернион слева: this * q
		 *  
		 * @param q множитель
		 */
		public function prepend(q:Quaternion):Quaternion {
			var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z;
			var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y;
			var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z;
			var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x;
			w = ww;
			x = xx;
			y = yy;
			z = zz;
			return this;
		}

		/**
		 * Умножает на указанный кватернион справа: q * this
		 *  
		 * @param q множитель
		 */
		public function append(q:Quaternion):Quaternion {
			var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z;
			var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y;
			var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z;
			var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x;
			w = ww;
			x = xx;
			y = yy;
			z = zz;
			return this;
		}

		/**
		 * 
		 * @param vector
		 */
		public function rotateByVector(v:Vector3):Quaternion {
			var ww:Number = -v.x*x - v.y*y - v.z*z;
			var xx:Number = v.x*w + v.y*z - v.z*y;
			var yy:Number = v.y*w + v.z*x - v.x*z;
			var zz:Number = v.z*w + v.x*y - v.y*x;
			w = ww;
			x = xx;
			y = yy;
			z = zz;
			return this;
		}

		/**
		 * Добавляет вращение, приданное вектором угловой скорости за указанное время.
		 * 
		 * @param v
		 * @param scale
		 */
		public function addScaledVector(v:Vector3, scale:Number):Quaternion {
			var vx:Number = v.x*scale;
			var vy:Number = v.y*scale;
			var vz:Number = v.z*scale;
			var ww:Number = -x*vx - y*vy - z*vz;
			var xx:Number = vx*w + vy*z - vz*y;
			var yy:Number = vy*w + vz*x - vx*z;
			var zz:Number = vz*w + vx*y - vy*x;
			w += 0.5*ww;
			x += 0.5*xx;
			y += 0.5*yy;
			z += 0.5*zz;
			// inlined normalize
			var d:Number = w*w + x*x + y*y + z*z;
			if (d == 0) {
				w = 1;
			} else {
				d = 1/Math.sqrt(d);
				w *= d;
				x *= d;
				y *= d;
				z *= d;
			}
			return this;
		}
		
		/**
		 * 
		 * @param m
		 * @return 
		 */
		public function toMatrix3(m:Matrix3):Quaternion {
			var xx2:Number = 2.0*x*x;
			var yy2:Number = 2.0*y*y;
			var zz2:Number = 2.0*z*z;
			var xy2:Number = 2.0*x*y;
			var yz2:Number = 2.0*y*z;
			var zx2:Number = 2.0*z*x;
			var wx2:Number = 2.0*w*x;
			var wy2:Number = 2.0*w*y;
			var wz2:Number = 2.0*w*z;
			
			m.a = 1.0 - yy2 - zz2;
			m.b = xy2 - wz2;
			m.c = zx2 + wy2;
			
			m.e = xy2 + wz2;
			m.f = 1.0 - xx2 - zz2;
			m.g = yz2 - wx2;
			
			m.i = zx2 - wy2;
			m.j = yz2 + wx2;
			m.k = 1.0 - xx2 - yy2;
			return this;
		}
		
		/**
		 * 
		 * @return 
		 */
		public function length():Number {
			return Math.sqrt(w*w + x*x + y*y + z*z);
		}

		/**
		 * 
		 * @return 
		 */
		public function lengthSqr():Number {
			return w*w + x*x + y*y + z*z;
		}
		
		/**
		 * 
		 * @param axis
		 * @param angle
		 * @return 
		 */		
		public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion {
			w = Math.cos(0.5*angle);
			var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z);
			x = axis.x*k;
			y = axis.y*k;
			z = axis.z*k;
			return this;
		}

		/**
		 * 
		 * @param x
		 * @param y
		 * @param z
		 * @param angle
		 */
		public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion {
			w = Math.cos(0.5*angle);
			var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z);
			this.x = x*k;
			this.y = y*k;
			this.z = z*k;
			return this;
		}
		
		/**
		 * 
		 * @param vector
		 */
		public function toAxisVector(v:Vector3 = null):Vector3 {
			if (w < -1 || w > 1) {
				normalize();
			}
			if (v == null) {
				v = new Vector3();
			}
			if (w > -1 && w < 1) {
				if (w == 0) {
					v.x = x;
					v.y = y;
					v.z = z;
				} else {
					var angle:Number = 2*Math.acos(w);
					var coeff:Number = 1/Math.sqrt(1 - w*w);
					v.x = x*coeff*angle;
					v.y = y*coeff*angle;
					v.z = z*coeff*angle;
				}
			} else {
				v.x = 0;
				v.y = 0;
				v.z = 0;
			}
			return v;
		}
		
		/**
		 * 
		 * @param rotations
		 */
		public function getEulerAngles(angles:Vector3):Vector3 {
			var qi2:Number = 2*x*x;
			var qj2:Number = 2*y*y;
			var qk2:Number = 2*z*z;
			var qij:Number = 2*x*y;
			var qjk:Number = 2*y*z;
			var qki:Number = 2*z*x;
			var qri:Number = 2*w*x;
			var qrj:Number = 2*w*y;
			var qrk:Number = 2*w*z;

			var aa:Number = 1 - qj2 - qk2;
			var bb:Number = qij - qrk;
			var ee:Number = qij + qrk;
			var ff:Number = 1 - qi2 - qk2;
			var ii:Number = qki - qrj;
			var jj:Number = qjk + qri;
			var kk:Number = 1 - qi2 - qj2;

			if (-1 < ii && ii < 1) {
				if (angles == null) {
					angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); 
				} else {
					angles.x = Math.atan2(jj, kk);
					angles.y = -Math.asin(ii);
					angles.z = Math.atan2(ee, aa);
				}
			} else {
				if (angles == null) {
					angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); 
				} else {
					angles.x = 0;
					angles.y = (ii <= -1) ? Math.PI : -Math.PI;
					angles.y *= 0.5;
					angles.z = Math.atan2(-bb, ff);
				}
			}
			return angles;
		}
		
		/**
		 * 
		 * @param x
		 * @param y
		 * @param z
		 */
		public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void {
			setFromAxisAngleComponents(1, 0, 0, x);
			
			_q.setFromAxisAngleComponents(0, 1, 0, y);
			append(_q);
			normalize();
			
			_q.setFromAxisAngleComponents(0, 0, 1, z);
			append(_q);
			normalize();
		}
		
		/**
		 * 
		 */
		public function conjugate():void {
			x = -x;
			y = -y;
			z = -z;
		}

		/**
		 * Выполняет линейную интерполяцию.
		 * 
		 * @param q1 начало отрезка
		 * @param q2 конец отрезка
		 * @param t время, обычно задаётся в интервале [0, 1]
		 */
		public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion {
			var d:Number = 1 - t;
			w = q1.w*d + q2.w*t;
			x = q1.x*d + q2.x*t;
			y = q1.y*d + q2.y*t;
			z = q1.z*d + q2.z*t;
			// inlined normalize
			d = w*w + x*x + y*y + z*z;
			if (d == 0) {
				w = 1;
			} else {
				d = 1/Math.sqrt(d);
				w *= d;
				x *= d;
				y *= d;
				z *= d;
			}
			return this;
		}
		
		/**
		 * 
		 * @param q
		 * @return 
		 */
		public function subtract(q:Quaternion):Quaternion {
			w -= q.w;
			x -= q.x;
			y -= q.y;
			z -= q.z;
			return this;
		}

		/**
		 * 
		 * @param q1
		 * @param q2
		 * @return 
		 */
		public function diff(q1:Quaternion, q2:Quaternion):Quaternion {
			w = q2.w - q1.w;
			x = q2.x - q1.x;
			y = q2.y - q1.y;
			z = q2.z - q1.z;
			return this;
		}
		
		/**
		 * 
		 * @param q
		 * @return 
		 */
		public function copy(q:Quaternion):Quaternion {
			w = q.w;
			x = q.x;
			y = q.y;
			z = q.z;
			return this;
		}
		
		/**
		 * @param reslult
		 */
		public function toVector3D(result:Vector3D):Vector3D {
			result.x = x;
			result.y = y;
			result.z = z;
			result.w = w;
			return result;
		}
		
		/**
		 * 
		 * @return 
		 */
		public function clone():Quaternion {
			return new Quaternion(w, x, y, z);
		}

		/**
		 * 
		 * @return 
		 */
		public function toString():String {
			return "[" + w + ", " + x + ", " + y + ", " + z + "]";
		}

		/**
		 * Выполняет сферическую интерполяцию между двумя заданными кватернионами по наименьшему расстоянию.
		 * 
		 * @param a первый кватерион
		 * @param b второй кватернион
		 * @param t параметр интерполяции, обычно принадлежит отрезку [0, 1]
		 * @return this
		 */
		public function slerp(a:Quaternion, b:Quaternion, t:Number):Quaternion {
			var flip:Number = 1;
			// Так как одна и та же ориентация представляется двумя значениями q и -q, нужно сменить знак одного из кватернионов
			// если скалярное произведение отрицательно. Иначе будет получено интерполированное значение по наибольшему расстоянию.
			var cosine:Number = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z;
			if (cosine < 0)	{ 
				cosine = -cosine; 
				flip = -1; 
			} 
			
			if ((1 - cosine) < 0.001) {
				// Вблизи нуля используется линейная интерполяция
				var k1:Number = 1 - t;
				var k2:Number = t*flip;
				w = a.w*k1 + b.w*k2;
				x = a.x*k1 + b.x*k2;
				y = a.y*k1 + b.y*k2;
				z = a.z*k1 + b.z*k2;
				normalize();
			} else {
				var theta:Number = Math.acos(cosine); 
				var sine:Number = Math.sin(theta); 
				var beta:Number = Math.sin((1 - t)*theta)/sine; 
				var alpha:Number = Math.sin(t*theta)/sine*flip;
				w = a.w*beta + b.w*alpha;
				x = a.x*beta + b.x*alpha;
				y = a.y*beta + b.y*alpha;
				z = a.z*beta + b.z*alpha;
			}
			
			return this; 
		} 

	}
}