using System;
using Tao.Platform;
using Tao.OpenGl;

namespace Kevin.Arcball
{
	/// <summary>
	/// Summary description for ArcBall.
	/// </summary>
	public class Arcball
	{
		private Vector3fT	stVec = new Vector3fT();	//Saved click vector
		private Vector3fT	enVec = new Vector3fT();	//Saved drag vector
		private float adjusM33idth = 2.0f;			//Mouse bounds width
		private float adjustHeight = 2.0f;			//Mouse bounds height

		public const double Epsilon = 1.0e-5;

		private bool mouseDown = false;

		public bool MouseDown
		{
			get
			{
				return this.mouseDown;
			}
		}

		public Arcball(float newWidth, float newHeight)
		{
			this.stVec.X = 0.0f;
			this.stVec.Y = 0.0f;
			this.stVec.Z = 0.0f;

			this.enVec.X = 0.0f;
			this.enVec.Y = 0.0f;
			this.enVec.Z = 0.0f;

			this.SetBounds(newWidth, newHeight);
		}

		public void SetBounds(float newWidth, float newHeight)
		{
			if (newWidth > 1.0f && newHeight > 1.0f)
			{
				adjusM33idth = 1.0f / ((newWidth - 1.0f) * 0.5f);
				adjustHeight = 1.0f / ((newHeight - 1.0f) * 0.5f);
			}
			else
			{
				adjusM33idth = 2.0f;
				adjustHeight = 2.0f;
			}
		}

		/// <summary>
		/// Gets the vector result of mapping of this point onto a sphere.
		/// </summary>
		/// <param name="adjusM33idth">Adjusted width</param>
		/// <param name="adjustHeight">Adjusted height</param>
		/// <returns>a resultant sphere-mapped vector</returns>
		public Vector3fT MapPointToSphere(float x, float y, float adjusM33idth, float adjustHeight)
		{
			Vector3fT newVec = new Vector3fT();

			//Adjust point coords and scale down to range of [-1 ... 1]
			x = (x * adjusM33idth) - 1.0f;
			y = 1.0f - (y * adjustHeight);

			//Compute the square of the length of the vector to the point from the center
			float length = (x*x) + (y*y);

			//If the point is mapped outside of the sphere... (length > radius squared)
			if (length > 1.0f)
			{
				float norm;

				//Compute a normalizing factor (radius / sqrt(length))
				norm = 1.0f / (float)Math.Sqrt(length);

				//set the vector to be a normalized point on the sphere
				newVec.X = x * norm;
				newVec.Y = y * norm;
				newVec.Z = 0.0f;
			}
			else //Else it's on the inside
			{
				//Return a vector to a point mapped inside the sphere sqrt(radius squared - length)
				newVec.X = x;
				newVec.Y = y;
				newVec.Z = (float)Math.Sqrt(1.0f - length);
			}

			return newVec;
		}


		/// <summary>
		/// On mouse down, maps point to sphere.
		/// </summary>
		/// <param name="newPt">The new point to map</param>
		public void Click(int x, int y)
		{
			stVec = MapPointToSphere(x, y, adjusM33idth, adjustHeight);
			this.mouseDown = true;
		}

		public void Release()
		{
			this.mouseDown = false;
		}

		// Drag
		public Quat4fT Drag(int x, int y)
		{
			Quat4fT newRot = new Quat4fT();

			this.enVec = MapPointToSphere(x, y, adjusM33idth, adjustHeight);

			//Compute the vector perpendicular to the begin and end vectors
			Vector3fT perp = Vector3fT.Cross(stVec, enVec);

			//Compute the length of the perpendicular vector
			if (perp.Length > Arcball.Epsilon) //if its non-zero
			{
				//We're ok, so return the perpendicular vector as the transform after all
				newRot.X = perp.X;
				newRot.Y = perp.Y;
				newRot.Z = perp.Z;

				//In the quaternion values, w is cosine (theta / 2), where theta is rotation angle
				newRot.W = Vector3fT.Dot(stVec, enVec);
			}
			else
			{
				//The begin and end vectors coincide, so return an identity transform
				newRot.X = 0.0f;
				newRot.Y = 0.0f;
				newRot.Z = 0.0f;
				newRot.W = 0.0f;
			}

			return newRot;
		}
	}

	/// <summary>
	/// A generic 2-element tuple that is represented by single-precision
	/// floating point x,y coordinates. 
	/// </summary>
	public class Tuple2fT
	{
		public float X;
		public float Y;

		/// <summary>
		/// Sets the value of this tuple to the vector difference of itself and tuple t1 (this = this - t1).
		/// </summary>
		/// <param name="a">This object</param>
		/// <param name="b">The target object</param>
		/// <returns></returns>
		public static Tuple2fT operator -(Tuple2fT a, Tuple2fT b)
		{
			Tuple2fT result = new Tuple2fT();

			result.X = a.X - b.X;
			result.Y = a.Y - b.Y;

			return result;
		}

		public static Tuple2fT operator +(Tuple2fT a, Tuple2fT b)
		{
			Tuple2fT result = new Tuple2fT();
			result.X = a.X + b.X;
			result.Y = a.Y + b.Y;
			return result;
		}

		public Tuple2fT()
		{
			this.X = 0.0f;
			this.Y = 0.0f;
		}
	}

	public class Tuple3fT : Tuple2fT
	{
		public float Z = 0.0f;

		public static Tuple3fT operator-(Tuple3fT a, Tuple4fT b)
		{
			Tuple3fT result = new Tuple3fT();
			result.X = a.X - b.X;
			result.Y = a.Y - b.Y;
			result.Z = a.Z - b.Z;
			return result;
		}
	}

	public class Tuple4fT : Tuple3fT
	{
		public float W = 0.0f;
	}

	/// <summary>
	/// A 2 element point that is represented by single precision floating point x,y coordinates. 
	/// </summary>
	public class Point2fT : Tuple2fT
	{
	}

	/// <summary>
	/// A 4 element unit quaternion represented by single precision floating point x,y,z,w coordinates. 
	/// </summary>
	public class Quat4fT : Tuple4fT
	{
		
		
		/// <summary>
		/// Converts this quaternion into a 3x3 Matrix
		/// </summary>
		public Matrix3fT Get3x3RotationMatrix
		{
			get
			{
				Matrix3fT newObj = new Matrix3fT();

				float n, s;
				float xs, ys, zs;
				float wx, wy, wz;
				float M00, M10, M20;
				float M11, M21, M22;

				n = (this.X * this.X) + (this.Y * this.Y) + (this.Z * this.Z) + (this.W * this.W);
				s = (n > 0.0f) ? (2.0f / n) : 0.0f;

				xs = this.X * s;  ys = this.Y * s;  zs = this.Z * s;
				wx = this.W * xs; wy = this.W * ys; wz = this.W * zs;
				M00 = this.X * xs; M10 = this.X * ys; M20 = this.X * zs;
				M11 = this.Y * ys; M21 = this.Y * zs; M22 = this.Z * zs;

				newObj.M00 = 1.0f - (M11 + M22); newObj.M01 =         M10 - wz;  newObj.M02 =         M20 + wy;
				newObj.M10 =         M10 + wz;  newObj.M11 = 1.0f - (M00 + M22); newObj.M12 =         M21 - wx;
				newObj.M20 =         M20 - wy;  newObj.M21 =         M21 + wx;  newObj.M22 = 1.0f - (M00 + M11);

				return newObj;
			}
		}
	}

	/// <summary>
	/// A 2-element vector that is represented by single-precision floating point x,y coordinates. 
	/// </summary>
	public class Vector2fT : Tuple2fT
	{
	}

	/// <summary>
	/// A 3-element vector that is represented by single-precision floating point x,y,z coordinates. 
	/// </summary>
	public class Vector3fT : Tuple3fT
	{
		/// <summary>
		/// Returns the result of the cross product operation A cross B.
		/// </summary>
		/// <param name="A">Vector A</param>
		/// <param name="B">Vector B</param>
		/// <returns>The cross product of A and B</returns>
		public static Vector3fT Cross(Vector3fT A, Vector3fT B)
		{
			Vector3fT R = new Vector3fT();

			R.X = (A.Y * B.Z) - (A.Z * B.Y);
			R.Y = (A.Z * B.X) - (A.X * B.Z);
			R.Z = (A.X * B.Y) - (A.Y * B.X);

			return R;
		}

		/// <summary>
		/// Returns the dot poduct of vectors A and B
		/// </summary>
		/// <param name="A">Vector A</param>
		/// <param name="B">Vector B</param>
		/// <returns>The dot product of A dot B</returns>
		public static float Dot(Vector3fT A, Vector3fT B)
		{
			return ((A.X * B.X) + (A.Y * B.Y) + (A.Z * B.Z));
		}

/*
		/// <summary>
		/// Returns the dot product of this vector, and vector B
		/// </summary>
		/// <param name="B">Vector B</param>
		/// <returns>The dot product of This dot B</returns>
		public float Dot(Vector3fT B)
		{
			return ((this.X * B.X) + (this.Y * B.Y) + (this.Z * B.Z));
		}*/

		/// <summary>
		/// The squared length of this vector.
		/// </summary>
		public float LengthSquared
		{
			get
			{
				return ((this.X * this.X) + (this.Y * this.Y) + (this.Z * this.Z));
			}
		}

		/// <summary>
		/// The square root of the squared length of this vector.
		/// </summary>
		public float Length
		{
			get
			{
				return ((float)Math.Sqrt(this.LengthSquared));
			}
		}
	}


	/// <summary>
	/// A single precision floating point 3 by 3 matrix. 
	/// </summary>
	public class Matrix3fT
	{
		//column major
		// column major
		public float M00 = 0.0f;	public float SX = 0.0f;		// XAxis.x and Scale X
		public float M10 = 0.0f;								// XAxis.Y
		public float M20 = 0.0f;								// XAxis.Z
		public float M01 = 0.0f;								// YAxis.X
		public float M11 = 0.0f;	public float SY = 0.0f;		// YAxis.Y and Scale Y
		public float M21 = 0.0f;								// YAxis.Z
		public float M02 = 0.0f;								// ZAxis.X
		public float M12 = 0.0f;								// ZAxis.Y
		public float M22 = 0.0f;	public float SZ = 0.0f;		// ZAxis.Z and Scale Z

		/// <summary>
		/// Returns an array suitable for using with OpenGL functions
		/// </summary>
		public float [] ToGLArray
		{
			get
			{
				float [] M = new float[9];
				M[0] = M00;
				M[1] = M10;
				M[2] = M20;
				M[3] = M01;
				M[4] = M11;
				M[5] = M21;
				M[6] = M02;
				M[7] = M12;
				M[8] = M22;
				return M;
			}
		}

		/// <summary>
		/// Sets this vector's variables to zero.
		/// </summary>
		public void Zero()
		{
			this.M00 = 0.0f;
			this.M01 = 0.0f;
			this.M02 = 0.0f;
			this.M10 = 0.0f;
			this.M11 = 0.0f;
			this.M12 = 0.0f;
			this.M20 = 0.0f;
			this.M21 = 0.0f;
			this.M22 = 0.0f;
		}

		/// <summary>
		/// Sets this matrix to an identity matrix.
		/// </summary>
		public void LoadIdentity()
		{
			Zero();

			this.M00 = 1.0f;
			this.M11 = 1.0f;
			this.M22 = 1.0f;
		}

		/// <summary>
		/// Returns a Matrix that is a conversion of This matrix with Quaternion Q
		/// </summary>
		/// <param name="Q">The quaternion to be converted to a resultant matrix.</param>
		/// <returns>The resultant matrix, converted from Quaternion Q</returns>
		/// <remarks>This can be optimized some (if s == 0)</remarks>
		public Matrix3fT SetRotationFromQuaternion(Quat4fT Q)
		{
			Matrix3fT R = new Matrix3fT();
			
			float n, s;
			float xs, ys, zs;
			float wx, wy, wz;
			float M00, M10, M20;
			float M11, M21, M22;

			n = (Q.X * Q.X) + (Q.Y * Q.Y) + (Q.Z * Q.Z) + (Q.W * Q.W);
			s = (n > 0.0f) ? (2.0f / n) : 0.0f;

			xs = Q.X * s;	ys = Q.Y * s;	zs = Q.Z * s;
			wx = Q.W * xs;	wy = Q.W * ys;	wz = Q.W * zs;
			M00 = Q.X * xs;	M10 = Q.X * ys;	M20 = Q.X * zs;
			M11 = Q.Y * ys;	M21 = Q.Y * zs;	M22 = Q.Z * zs;

			R.M00 = 1.0f - (M11 + M22);	R.M01 = M10 - wz;				R.M02 = M20 + wy;
			R.M10 = M10 + wz;				R.M11 = 1.0f - (M00 + M22);	R.M12 = M21 - wx;
			R.M20 = M20 - wy;				R.M21 = M21 + wx;				R.M22 = 1.0f - (M00 + M11);

			return R;
		}

		/// <summary>
		/// Returns a matrix that is the result of This Matrix multiplied by Matrix B
		/// </summary>
		/// <param name="B">Matrix B</param>
		/// <returns>Matrix R = This Matrix multiplied by Matrix B</returns>
		public static Matrix3fT operator*(Matrix3fT A, Matrix3fT B)
		{
			Matrix3fT R = new Matrix3fT();

			R.M00 = (A.M00 * B.M00) + (A.M01 * B.M10) + (A.M02 * B.M20);
			R.M01 = (A.M00 * B.M01) + (A.M01 * B.M11) + (A.M02 * B.M21);
			R.M02 = (A.M00 * B.M02) + (A.M01 * B.M12) + (A.M02 * B.M22);

			R.M10 = (A.M10 * B.M00) + (A.M11 * B.M10) + (A.M12 * B.M20);
			R.M11 = (A.M10 * B.M01) + (A.M11 * B.M11) + (A.M12 * B.M21);
			R.M12 = (A.M10 * B.M02) + (A.M11 * B.M12) + (A.M12 * B.M22);

			R.M20 = (A.M20 * B.M00) + (A.M21 * B.M10) + (A.M22 * B.M20);
			R.M21 = (A.M20 * B.M01) + (A.M21 * B.M11) + (A.M22 * B.M21);
			R.M22 = (A.M20 * B.M02) + (A.M21 * B.M12) + (A.M22 * B.M22);

			return R;
		}
	}

	/// <summary>
	/// A single precision public floating point 4 by 4 matrix. 
	/// </summary>
	public class Matrix4fT
	{
		public float M00 = 0.0f;	public float SX = 0.0f;		// XAxis.X and Scale X
		public float M10 = 0.0f;								// XAxis.Y
		public float M20 = 0.0f;								// XAxis.Z
		public float M30 = 0.0f;								// XAxis.W
		public float M01 = 0.0f;								// YAxis.X
		public float M11 = 0.0f;	public float SY = 0.0f;		// YAxis.Y and Scale Y
		public float M21 = 0.0f;								// YAxis.Z
		public float M31 = 0.0f;								// YAxis.W
		public float M02 = 0.0f;								// ZAxis.X
		public float M12 = 0.0f;								// ZAxis.Y
		public float M22 = 0.0f;	public float SZ = 0.0f;		// ZAxis.Z and Scale Z
		public float M32 = 0.0f;								// ZAxis.W
		public float M03 = 0.0f;								// Trans.X
		public float M13 = 0.0f;								// Trans.Y
		public float M23 = 0.0f;								// Trans.Z
		public float M33 = 0.0f;	public float SW = 0.0f;		// Trans.W and Scale W

		/// <summary>
		/// Returns an array suitable for using with OpenGL functions
		/// </summary>
		public float [] ToGLArray
		{
			get
			{
				float [] M = new float[16];
				M[0] = M00;
				M[1] = M10;
				M[2] = M20;
				M[3] = M30;
				M[4] = M01;
				M[5] = M11;
				M[6] = M21;
				M[7] = M31;
				M[8] = M02;
				M[9] = M12;
				M[10] = M22;
				M[11] = M32;
				M[12] = M03;
				M[13] = M13;
				M[14] = M23;
				M[15] = M33;
				return M;
			}
		}
	
		/// <summary>
		/// Returns This matrix with the rotation scale of Matrix B
		/// </summary>
		/// <param name="B">Matrix B</param>
		/// <returns>A new matrix, with the rotation scale of Matrix B</returns>
		public static void SetRotationScaleFrom(ref Matrix4fT A, Matrix4fT B)
		{
			A.M00 = B.M00;	A.M01 = B.M01;	A.M02 = B.M02;
			A.M10 = B.M10;	A.M11 = B.M11;	A.M12 = B.M12;
			A.M20 = B.M20;	A.M21 = B.M21;	A.M22 = B.M22;
		}

		/// <summary>
		/// Sets this vector's variables to zero.
		/// </summary>
		public void Zero()
		{
			this.M00 = 0.0f;
			this.M01 = 0.0f;
			this.M02 = 0.0f;
			this.M03 = 0.0f;
			this.M10 = 0.0f;
			this.M11 = 0.0f;
			this.M12 = 0.0f;
			this.M13 = 0.0f;
			this.M20 = 0.0f;
			this.M21 = 0.0f;
			this.M22 = 0.0f;
			this.M23 = 0.0f;
			this.M30 = 0.0f;
			this.M31 = 0.0f;
			this.M32 = 0.0f;
			this.M33 = 0.0f;
		}

		/// <summary>
		/// Sets this matrix to an identity matrix.
		/// </summary>
		public void LoadIdentity()
		{
			Zero();

			this.M00 = 1.0f;
			this.M11 = 1.0f;
			this.M22 = 1.0f;
			this.M33 = 1.0f;
		}

		public float SingularValueDecomposition()
		{
			Matrix3fT n = null;
			return SingularValueDecomposition(ref n, ref n);
		}

		/// <summary>
		/// Performs singular-value decomposition on this matrix, and gets scale and rotation. Rotation 
		/// </summary>
		/// <param name="rot3"></param>
		/// <param name="rot4"></param>
		/// <returns>The scale factor</returns>
		public float SingularValueDecomposition(ref Matrix3fT rot3, ref Matrix3fT rot4)
		{
			float s;
			float n;

			s = (float) Math.Sqrt( (
				(this.M00 * this.M00) + (this.M10 * this.M10) + (this.M20 * this.M20) +
				(this.M01 * this.M01) + (this.M11 * this.M11) + (this.M21 * this.M21) +
				(this.M02 * this.M02) + (this.M12 * this.M12) + (this.M22 * this.M22) ) / 3.0f );

			if (rot3 != null)
			{
				rot3.M00 = this.M00;	rot3.M10 = this.M10;	rot3.M20 = this.M20;
				rot3.M01 = this.M01;	rot3.M11 = this.M11;	rot3.M21 = this.M21;
				rot3.M02 = this.M02;	rot3.M12 = this.M12;	rot3.M22 = this.M22;

				//try // catch division by zero

				n = 1.0f / (float)Math.Sqrt( (this.M00 * this.M00) + (this.M10 * this.M10) + (this.M20 * this.M20) );
				rot3.M00 *= n;
				rot3.M10 *= n;
				rot3.M20 *= n;

				n = 1.0f / (float)Math.Sqrt( (this.M01 * this.M01) + (this.M11 * this.M11) + (this.M21 * this.M21) );
				rot3.M01 *= n;
				rot3.M11 *= n;
				rot3.M21 *= n;

				n = 1.0f / (float)Math.Sqrt( (this.M02 * this.M02) + (this.M12 * this.M12) + (this.M22 * this.M22) );
				rot3.M02 *= n;
				rot3.M12 *= n;
				rot3.M22 *= n;
			}

			if (rot4 != null)
			{
				//Matrix4fSetRotationScaleFromMatrix4f(rot4, NewObj);  // private method

				// try / catch division by zero

				n = 1.0f / (float)Math.Sqrt( (this.M00 * this.M00) + (this.M10 * this.M10) + (this.M20 * this.M20) );
				rot4.M00 *= n;
				rot4.M10 *= n;
				rot4.M20 *= n;

				n = 1.0f / (float)Math.Sqrt( (this.M01 * this.M01) + (this.M11 * this.M11) + (this.M21 * this.M21) );
				rot4.M01 *= n;
				rot4.M11 *= n;
				rot4.M21 *= n;

				n = 1.0f / (float)Math.Sqrt( (this.M02 * this.M02) + (this.M12 * this.M12) + (this.M22 * this.M22) );
				rot4.M02 *= n;
				rot4.M12 *= n;
				rot4.M22 *= n;
			}

			return s;
		}

		/// <summary>
		/// Modifies this matrix a Matrix4fT that is Matrix A, with the rotation scale from Matrix B
		/// </summary>
		/// <param name="A">Matrix A</param>
		/// <param name="B">Matrix B</param>
		/// <remarks>Modifies This</remarks>
		public void SetRotationScaleFrom(Matrix3fT B)
		{
			this.M00 = B.M00; this.M01 = B.M01; this.M02 = B.M02;
			this.M10 = B.M10; this.M11 = B.M11; this.M12 = B.M12;
			this.M20 = B.M20; this.M21 = B.M21; this.M22 = B.M22;
		}

		/// <summary>
		/// Modifies this matrix. Scales the matrix by "scale"
		/// </summary>
		/// <param name="scale">The scaling factor</param>
		public void MultiplyRotationScale(float scale)
		{
			this.M00 *= scale; this.M01 *= scale; this.M02 *= scale;
			this.M10 *= scale; this.M11 *= scale; this.M12 *= scale;
			this.M20 *= scale; this.M21 *= scale; this.M22 *= scale;
		}

		/// <summary>
		/// Sets the rotational component (upper 3x3) of this matrix to the matrix
		/// values in the T precision Matrix3d argument; the other elements of
		/// this matrix are unchanged; a singular value decomposition is performed
		/// on this object's upper 3x3 matrix to factor out the scale, then this
		/// object's upper 3x3 matrix components are replaced by the passed rotation
		/// components, and then the scale is reapplied to the rotational
		/// components.
		/// </summary>
		/// <param name="B">The other 3x3 matrix</param>
		/// <returns>A 4x4 Matrix</returns>
		public void SetRotationFromMatrix3f(Matrix3fT B)
		{
			float scale = this.SingularValueDecomposition();
			this.SetRotationScaleFrom(B);
			this.MultiplyRotationScale(scale);
		}
	}
}
