using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Xml.Serialization;
using System.Runtime.InteropServices;

namespace VisiMath
{
    /// <summary>
    /// Quaternion structure used for storing rotation data in 3D
    /// </summary>
    [Serializable]
    [StructLayout(LayoutKind.Sequential)]
    public struct Quaternion : IEquatable<Quaternion>, IXmlSerializable
    {
        #region Fields

        Vector4 vec;

        /// <summary>
        /// Complex value 1
        /// </summary>
        public float X
        {
            get { return vec.X; }

            set { vec.X = value; }
        }
        /// <summary>
        /// Complex value 2
        /// </summary>
        public float Y
        {
            get { return vec.Y; }

            set { vec.Y = value; }
        }
        /// <summary>
        /// Complex value 3
        /// </summary>
        public float Z
        {
            get { return vec.Z; }

            set { vec.Z = value; }
        }
        /// <summary>
        /// Real value
        /// </summary>
        public float W
        {
            get { return vec.W; }

            set { vec.W = value; }
        }

        #endregion

        #region Constructors

        public Quaternion(float x, float y, float z, float w)
        {
            vec = new Vector4(x, y, z, w);
        }

        public Quaternion(Vector3 xv, Vector3 yv, Vector3 zv)
        {
            this = CreateFromMatrix(xv.X, xv.Y, xv.Z, yv.X, yv.Y, yv.Z, zv.X, zv.Y, zv.Z);
        }

        public Quaternion(Matrix3 mat)
        {
            this = CreateFromMatrix(ref mat);
        }



        #endregion

        #region Constants

        /// <summary>
        /// Identity quaternion.
        /// </summary>
        public static Quaternion Identity = new Quaternion(0, 0, 0, 1);

        #endregion

        #region Methods

        public float Length
        {
            get
            {
                return vec.Length;
            }
        }

        public float LengthSquared
        {
            get
            {
                return vec.LengthSquared;
            }
        }


        /// <summary>
        /// Converts Quaternion to Axis and angle representation
        /// </summary>
        /// <param name="axis">Vector to store axis</param>
        /// <param name="angle">float to store angle</param>
        public void GetAxisAngle(out Vector3 axis, out float angle)
        {

            if (W > 1.0f)
                Normalize();

            angle = 2.0f * (float)System.Math.Acos(W);

            float den = (float)System.Math.Sqrt(1.0 - W * W);
            if (den > 0.0001f)
            {
                float inden = 1.0f/den;
                axis = new Vector3(X * inden, Y*inden, Z*inden);
            }
            else
            {
                axis = new Vector3(1, 0, 0);
            }        
        }

        /// <summary>
        /// Returns rotational matrix3x3 (expects normalized quaternion)
        /// </summary>
        /// <returns></returns>
        public Matrix3 GetRotationMatrix()
        {
            /*
            Vector3 axis;
            float angle;
            GetAxisAngle(out axis, out angle);
            Matrix3 rval1 = Matrix3.CreateRotationAxisAngle(axis, angle);
             */

            float xx = X * X;
            float yy = Y * Y;
            float zz = Z * Z;
            float xy = X * Y;
            float xz = X * Z;
            float yz = Y * Z;
            float wx = W * X;
            float wy = W * Y;
            float wz = W * Z;

            return new Matrix3(
                1 - 2 * (yy + zz), 2 * (xy + wz), 2 * (xz - wy),
                2 * (xy - wz), 1 - 2 * (xx + zz), 2 * (yz + wx),
                2 * (xz + wy), 2 * (yz - wx), 1 - 2 * (xx + yy));
        }

        /// <summary>
        /// Returns transformation matrix
        /// </summary>
        /// <returns>Rotational matrix </returns>
        public Matrix4 GetMatrix()
        {
            Matrix3 mm = GetRotationMatrix();
            return new Matrix4(mm);
        }

        /// <summary>
        /// Converts quaternion to rotation in Roll-Pitch-Yaw format
        /// </summary>
        /// <returns>RYP rotation</returns>
        public Vector3 GetRPY()
        {
            double ww = W*W;
			double xx = X*X;
			double yy = Y*Y;
			double zz = Z*Z;

            return new Vector3(
                (float)Math.Atan2(2*(Y*Z + W*X), ww - xx - yy + zz),
                (float)Math.Asin(-2 * (X * Z - W * Y)),
                (float)Math.Atan2(2 * (X * Y + W * Z), ww + xx - yy - zz));
        }


        /// <summary>
        /// Normalizes quaternion;
        /// </summary>
        public void Normalize()
        {
            vec.Normalize();
        }

        public void Multiply(ref Quaternion right)
        {
            right.vec.Assign(
                W * right.X + X * right.W + Y * right.Z - Z * right.Y, 
                W * right.Y + Y * right.W + Z * right.X - X * right.Z, 
                W * right.Z + Z * right.W + X * right.Y - Y * right.X,
                W * right.W - X * right.X - Y * right.Y - Z * right.Z
                );
        }

        public Vector3 Multiply(ref Vector3 right)
        {
            float xx = X * X;
            float yy = Y * Y;
            float zz = Z * Z;
            float xy = X * Y;
            float xz = X * Z;
            float yz = Y * Z;
            float wx = W * X;
            float wy = W * Y;
            float wz = W * Z;

            //note: optimize more?
            return new Vector3(
                (  (1 - 2 * (yy + zz)) * right.X + 2 * (xy + wz) * right.Y + 2 * (xz - wy) * right.Z ) ,
                ( 2 * (xy - wz) * right.X + (1 - 2 * (xx + zz))*right.Y + 2 * (yz + wx)*right.Z ),
                ( 2 * (xz + wy) * right.X + 2 * (yz - wx) * right.Y + (1 - 2 * (xx + yy))*right.Z) );
        }

        /// <summary>
        /// Rotates quaternion by scaled vector
        /// </summary>
        /// <param name="vec">Vector rotation</param>
        /// <param name="scale">Scale</param>
        public void AddVector(ref Vector3 vec, float scale = 1.0f)
        {
            Quaternion q = new Quaternion(vec.X * scale, vec.Y * scale, vec.Z * scale, 0);
            q = q * this;

            X += q.X * 0.5f;
            Y += q.Y * 0.5f;
            Z += q.Z * 0.5f;
            W += q.W * 0.5f;
        }


        //note, constained quaternions based on Constrained Quaternions by David Eberly

        /// <summary>
        /// Constraints rotation, returns closest quaternion for rotation limited around the x axis
        /// </summary>
        /// <returns>Constrained quaternion</returns>
        public Quaternion ClosestX()
        {
            return ClosestSingleAxis(0);
        }

        /// <summary>
        /// Constraints rotation, returns closest quaternion for rotation limited around the Y axis
        /// </summary>
        /// <returns>Constrained quaternion</returns>
        public Quaternion ClosestY()
        {
            return ClosestSingleAxis(1);
        }

        /// <summary>
        /// Constraints rotation, returns closest quaternion for rotation limited around the Z axis
        /// </summary>
        /// <returns>Constrained quaternion</returns>
        public Quaternion ClosestZ()
        {
            return ClosestSingleAxis(2);
        }

        /// <summary>
        /// Constraints rotation, returns closest quaternion for rotation limited around single axis (0 =x, 1= y...)
        /// </summary>
        /// <returns>Constrained quaternion</returns>
        public Quaternion ClosestSingleAxis(int axis)
        {
            float sqrLength = W * W + vec[axis] * vec[axis];
            if (sqrLength > 0)
            {
                float inv = 1.0f / sqrLength;
                Quaternion rval = new Quaternion();
                rval.W = W * inv;
                rval.vec[axis] = vec[axis] * inv;
                
                return rval;
            }
            else
            {
                return Quaternion.Identity;
            }
        }

        /// <summary>
        /// Constraints rotation, returns closest quaternion for rotation limited around the Y axis and then x axis
        /// </summary>
        /// <returns>Constrained quaternion</returns>
        public Quaternion ClosestXY()
        {
            return ClosestXY(this);
        }
        /// <summary>
        /// Constraints rotation, returns closest quaternion for rotation limited around the X axis and then Y axis
        /// </summary>
        /// <returns>Constrained quaternion</returns>
        public Quaternion ClosestYX()
        {
            return ClosestYX(this);
        }

        /// <summary>
        /// Constraints rotation, returns closest quaternion for rotation limited around the Z axis and then X axis
        /// </summary>
        /// <returns>Constrained quaternion</returns>
        public Quaternion ClosestXZ()
        {
            return ClosestXZ(this);
        }
        
        /// <summary>
        /// Constraints rotation, returns closest quaternion for rotation limited around the X axis and then Z axis
        /// </summary>
        /// <returns>Constrained quaternion</returns>
        public Quaternion ClosestZX()
        {
            return ClosestZX(this);
        }

        /// <summary>
        /// Constraints rotation, returns closest quaternion for rotation limited around the Z axis and then Y axis
        /// </summary>
        /// <returns>Constrained quaternion</returns>
        public Quaternion ClosestYZ()
        {
            return ClosestYZ(this);
        }

        /// <summary>
        /// Constraints rotation, returns closest quaternion for rotation limited around the Y axis and then Z axis
        /// </summary>
        /// <returns>Constrained quaternion</returns>
        public Quaternion ClosestZY()
        {
            return ClosestZY(this);
        }


        /// <summary>
        /// Constraints rotation, returns closest quaternion for rotation limited around the Y axis and then x axis
        /// </summary>
        /// <param name="q">Original quaternion</param>
        /// <returns>Constrained quaternion</returns>
        public static Quaternion ClosestXY(Quaternion q)
        {
            float det = q.W * q.Z - q.X * q.Y;
            if (MathTools.Abs(det) < 0.5f)
            {
                float discr = MathTools.Sqrt(1 - 4 * det * det);
                float a = q.W * q.X + q.Y * q.Z;
                float b = q.W * q.W - q.X * q.X + q.Y * q.Y - q.Z * q.Z;
                float c0, s0;
                if (b >= 0)
                {
                    c0 = 0.5f * (discr + b);
                    s0 = a;
                }
                else
                {
                    c0 = a;
                    s0 = 0.5f * (discr - b);
                }
                float invlength = 1.0f / MathTools.Sqrt(c0 * c0 + s0 * s0);
                c0 *= invlength;
                s0 *= invlength;
                float c1 = q.W * c0 + q.X * s0;
                float s1 = q.Y * c0 + q.Z * s0;
                invlength = 1.0f / MathTools.Sqrt(c1 * c1 + s1 * s1);
                c1 *= invlength;
                s1 *= invlength;
                return new Quaternion(s0 * c1, c0 * s1, s0 * s1, c0 * c1);
            }
            else
            {
                float invlength = 1.0f / MathTools.Sqrt(MathTools.Abs(det));
                return new Quaternion(q.X * invlength, 0, 0, q.W * invlength);
            }
        }

        /// <summary>
        /// Constraints rotation, returns closest quaternion for rotation limited around the X axis and then Y axis
        /// </summary>
        /// <param name="q">Original quaternion</param>
        /// <returns>Constrained quaternion</returns>
        public static Quaternion ClosestYX(Quaternion q)
        {
            Quaternion rval = ClosestXY(new Quaternion(q.X, q.Y, -q.Z, q.W));
            rval.Z = -rval.Z;
            return rval;
        }

        /// <summary>
        /// Constraints rotation, returns closest quaternion for rotation limited around the Z axis and then X axis
        /// </summary>
        /// <param name="q">Original quaternion</param>
        /// <returns>Constrained quaternion</returns>
        public static Quaternion ClosestZX(Quaternion q)
        {
            Quaternion rval = ClosestXY(new Quaternion(q.X, q.Z, q.Y, q.W));
            float tmp = rval.Y;
            rval.Y = rval.Z;
            rval.Z = tmp;
            return rval;            
        }

        /// <summary>
        /// Constraints rotation, returns closest quaternion for rotation limited around the X axis and then Z axis
        /// </summary>
        /// <param name="q">Original quaternion</param>
        /// <returns>Constrained quaternion</returns>
        public static Quaternion ClosestXZ(Quaternion q)
        {
            Quaternion rval = ClosestXY(new Quaternion(q.X, -q.Z, q.Y, q.W));
            float tmp = rval.Y;
            rval.Y = rval.Z;
            rval.Z = -tmp;
            return rval;
        }

        /// <summary>
        /// Constraints rotation, returns closest quaternion for rotation limited around the Z axis and then Y axis
        /// </summary>
        /// <param name="q">Original quaternion</param>
        /// <returns>Constrained quaternion</returns>
        public static Quaternion ClosestYZ(Quaternion q)
        {
            Quaternion rval = ClosestXY(new Quaternion(q.Y, q.Z, q.X, q.W));
            float tmp = rval.Z;
            rval.Z = rval.Y;
            rval.Y = rval.X;
            rval.X = tmp;
            return rval;
        }

        /// <summary>
        /// Constraints rotation, returns closest quaternion for rotation limited around the Y axis and then Z axis
        /// </summary>
        /// <param name="q">Original quaternion</param>
        /// <returns>Constrained quaternion</returns>
        public static Quaternion ClosestZY(Quaternion q)
        {
            Quaternion rval = ClosestXY(new Quaternion(q.Y, q.Z, -q.X, q.W));
            float tmp = rval.Z;
            rval.Z = rval.Y;
            rval.Y = rval.X;
            rval.X = -tmp;
            return rval;
        }

        #endregion

        #region Static methods

        /// <summary>
        /// Creates quaternion from the rotational matrix.
        /// Note: may fail if rotational part contains scale / shearing etc
        /// </summary>
        /// <returns>new quaternion</returns>
        public static Quaternion CreateFromMatrix(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22)
        {

			float root;
			float trace = m00 + m11 + m22;
			if (trace > 0.0)
			{
				root=(float)Math.Sqrt(trace+1.0f);
				float w=0.5f*root;
				root = 0.5f/root;
				float x = (m21-m12)*root;
				float y = (m02-m20)*root;
				float z = (m10-m01)*root;
                return new Quaternion(x, y, z, w);
			}
			else 
			{
	            if ((m00 > m11) && (m00 > m22))
                {
                    float s = 2.0f * (float)Math.Sqrt(1.0f + m00 - m11 - m22);
                    float ins = 1.0f / s;
                    return new Quaternion(
                        0.25f * s, (m01 + m10) * ins, (m02 + m20) * ins, (m21 - m12) * ins);
                }
                else if (m11 > m12)
                {
                    float s = 2.0f * (float)Math.Sqrt( 1.0f + m11 - m00 - m22);
                    float ins = 1.0f / s;
                    return new Quaternion(
                      (m01 + m10) * ins, 0.25f * s, (m12 + m21) * ins, (m02 - m20) * ins);
                }
                else
                {
                    float s = 2.0f * (float)Math.Sqrt( 1.0f + m22 - m00 - m11 );
                    float ins = 1.0f / s;
                    return new Quaternion(
                      (m02 + m20 ) * ins, (m12 + m21 ) * ins, 0.25f * s, (m10 - m01 ) * ins);
                }
			}        
        }

        /// <summary>
        /// Creates quaternion from the rotational matrix.
        /// Note: may fail if rotational part contains scale / shearing etc
        /// </summary>
        /// <param name="mat">source matrix</param>
        /// <returns>new quaternion</returns>
        public static Quaternion CreateFromMatrix(ref Matrix3 matrix)
        {
            return CreateFromMatrix(matrix.m00, matrix.m01, matrix.m02, matrix.m10, matrix.m11, matrix.m12, matrix.m20, matrix.m21, matrix.m22);
        }

        /// <summary>
        /// Creates quaternion from the rotational part of the matrix.
        /// Note: may fail if rotational part contains scale / shearing etc
        /// </summary>
        /// <param name="mat">source matrix</param>
        /// <returns>new quaternion</returns>
        public static Quaternion CreateFromMatrix(ref Matrix4 matrix)
        {
            return CreateFromMatrix(matrix.m00, matrix.m01, matrix.m02, matrix.m10, matrix.m11, matrix.m12, matrix.m20, matrix.m21, matrix.m22);
        }


        /// <summary>
        /// Creates quaternion from the rotation around the axis
        /// </summary>
        /// <param name="axis">Axis of rotation</param>
        /// <param name="angle">Angle in radians</param>
        /// <returns>New quaternion</returns>
        public static Quaternion CreateFromAxisAngle(ref Vector3 axis, float angle)
        {
            float halfangle = 0.5f * angle;
            float sn = (float)Math.Sin(halfangle);
            return new Quaternion(sn * axis.X, sn * axis.Y, sn * axis.Z, (float)Math.Cos(halfangle));        
        }

        /// <summary>
        /// Creates quaternion from the rotation around the axis
        /// </summary>
        /// <param name="axis">Axis of rotation</param>
        /// <param name="angle">Angle in radians</param>
        /// <returns>New quaternion</returns>
        public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle)
        {
            return CreateFromAxisAngle(ref axis, angle);
        }

        /// <summary>
        /// Creates new Quaternion from the rotation in Roll Pitch Yaw format
        /// </summary>
        /// <param name="rpy">Rotation Vector x = roll, y = pitch, z = yaw</param>
        /// <returns>New quaternion</returns>
        public static Quaternion CreateFromRPY(ref Vector3 rpy)
        {
            return CreateFromRPY(rpy.X, rpy.Y, rpy.Z);
        }

        public static Quaternion LookAt(Vector3 dir, Vector3 up)
        {
            Vector3 zAxis = dir;
            Vector3 xAxis = up.Cross(ref zAxis); xAxis.Normalize();
            Vector3 yAxis = zAxis.Cross(ref xAxis); yAxis.Normalize();
            return new Quaternion(zAxis, xAxis, yAxis);
        }


        public static Quaternion CreateFromRPY(float roll, float pitch, float yaw)
        {
       /*     Quaternion qyaw = CreateFromAxisAngle(Vector3.UnitZ, yaw);

            Matrix3 mat = qyaw.GetRotationMatrix();

            Vector3 axis;
            float angle;
            qyaw.GetAxisAngle(out axis, out angle);
            Matrix3 rmm = Matrix3.CreateRotationAxisAngle(axis, angle);
            Matrix3 rm = Matrix3.CreateRotationZ(yaw);



            Quaternion qpitch = CreateFromAxisAngle(Vector3.UnitY, pitch);
            mat = qpitch.GetRotationMatrix();
            qpitch.GetAxisAngle(out axis, out angle);
            rmm = Matrix3.CreateRotationAxisAngle(axis, angle);
            rm = Matrix3.CreateRotationY(pitch);



            Quaternion qroll = CreateFromAxisAngle(Vector3.UnitX, roll);
            mat = qroll.GetRotationMatrix();
            qroll.GetAxisAngle(out axis, out angle);
            rmm = Matrix3.CreateRotationAxisAngle(axis, angle);
            rm = Matrix3.CreateRotationX(roll);


            Quaternion rvalx = qroll * qpitch * qyaw;
            Quaternion rval = qyaw * qpitch * qroll;
            //Quaternion rval = qroll * qyaw;
            //rval.Normalize();
            
            
            */

            /*
            Quaternion qroll = Quaternion.CreateFromAxisAngle(Vector3.UnitX, roll);
            Quaternion qpitch = Quaternion.CreateFromAxisAngle(Vector3.UnitY, yaw);
            Quaternion qyaw = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, pitch);

            Quaternion rval = qroll * qpitch * qyaw;

            return rval;*/

            float cr = MathTools.Cos(roll / 2);
            float cp = MathTools.Cos(pitch / 2);
            float cy = MathTools.Cos(yaw / 2);

            float sr = MathTools.Sin(roll / 2);
            float sp = MathTools.Sin(pitch / 2);
            float sy = MathTools.Sin(yaw / 2);

            Quaternion rval2 = new Quaternion(
                sr * cp * cy - cr * sp * sy,
                cr * sp * cy + sr * cp * sy,
                cr * cp * sy - sr * sp * cy,
                cr * cp * cy + sr * sp * sy);

            return rval2;
        }


        #endregion

        #region Operators

        public static Quaternion operator +(Quaternion left, Quaternion right)
        {
            left.vec.Add(ref right.vec);
            return left;
        }

        public static Quaternion operator -(Quaternion left, Quaternion right)
        {
            left.vec.Substract(ref right.vec);
            return left;
        }

        public static Quaternion operator *(Quaternion left, Quaternion right)
        {
            left.Multiply(ref right);
            return right;
        }

        public static Quaternion operator *(Quaternion left, float scale)
        {
            left.vec.Mul(scale);
            return left;
        }

        public static Quaternion operator *(float scale, Quaternion left)
        {
            left.vec.Mul(scale);
            return left;
        }

        public static Vector3 operator *(Quaternion left, Vector3 right)
        {
            return left.Multiply(ref right);
        }


        public static bool operator ==(Quaternion left, Quaternion right)
        {
            return left.Equals(right);
        }

        public static bool operator !=(Quaternion left, Quaternion right)
        {
            return !left.Equals(right);
        }


        #endregion

        #region overrides

        public override int GetHashCode()
        {
            return vec.GetHashCode();
        }

        public override string ToString()
        {
            return vec.ToString();
        }

        public override bool Equals(object obj)
        {
            if (obj is Quaternion)
                return Equals((Quaternion)obj);
            return false;
        }

        public bool Equals(Quaternion other)
        {
            return vec.Equals(other.vec);
        }

        #endregion



        #region IXmlSerializable Members

        /// <summary>
        /// Reqired by IXMLserializable
        /// </summary>
        /// <returns>Null</returns>
        public System.Xml.Schema.XmlSchema GetSchema()
        {
            return null;
        }

        /// <summary>
        /// Deserializes object from XML
        /// </summary>
        /// <param name="reader">XML reader</param>
        public void ReadXml(System.Xml.XmlReader reader)
        {
            vec.ReadXml(reader);
        }

        /// <summary>
        /// Serializes object to XML
        /// </summary>
        /// <param name="writer">XML writer to save values</param>
        public void WriteXml(System.Xml.XmlWriter writer)
        {
            vec.WriteXml(writer);
        }

        /// <summary>
        /// Serializes object to XML
        /// </summary>
        /// <param name="writer">XML writer to save values</param>
        /// <param name="rootname">Node to save value</param>
        public void WriteXml(System.Xml.XmlWriter writer, string rootname)
        {
            writer.WriteStartElement(rootname);
            WriteXml(writer);
            writer.WriteEndElement();
        }

        #endregion
    }
}
