using System;
using WiimoteLib;

namespace Log740
{
    public class Quaternion
    {
        private double x, y, z, w;

        public static Quaternion Identity()
        {
            return new Quaternion(0, 0, 0, 1);
        }

        public static Quaternion FromEuler(double rotx, double roty, double rotz)
        {
            double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;

            ti = rotx * 0.5f;
            tj = roty * 0.5f;
            th = rotz * 0.5f;

            ci = Math.Cos(ti);
            cj = Math.Cos(tj);
            ch = Math.Cos(th);
            si = Math.Sin(ti);
            sj = Math.Sin(tj);
            sh = Math.Sin(th);

            cc = ci * ch;
            cs = ci * sh;
            sc = si * ch;
            ss = si * sh;

            Quaternion Q = new Quaternion(cj * sc - sj * cs, cj * ss + sj * cc, cj * cs - sj * sc, cj * cc + sj * ss);

            Q.Normalize();

            return Q;
        }

        public static Quaternion FromAxisAngle(double x, double y, double z, double angle)
        {
            double sin_angle = Math.Sin(angle / 2);

            x *= sin_angle;
            y *= sin_angle;
            z *= sin_angle;

            Quaternion Q = new Quaternion(x, y, z, Math.Cos(angle / 2));

            Q.Normalize();

            return Q;
        }

        public static Quaternion BetweenAxis(Vecteur3F A, Vecteur3F B)
        {
            Vecteur3F axis;
            double angle;

            axis = A.Cross(B);
            axis.Normalize();

            angle = A.Angle(B);

            return FromAxisAngle(axis.X, axis.Y, axis.Z, angle);
        }

        public Quaternion(double x, double y, double z, double w)
        {
            this.x = x;
            this.y = y;
            this.z = z;
            this.w = w;
        }

        public void Normalize()
        {
            double length = Math.Sqrt(x * x + y * y + z * z + w * w);
            this.x /= length;
            this.y /= length;
            this.z /= length;
            this.w /= length;
        }

        public Quaternion Inverted()
        {
            return new Quaternion(-x, -y, -z, w);
        }

        public Vecteur3F toEuler()
        {
            double x, y, z;

            x = Math.Atan(2 * (this.w * this.x + this.y * this.z) / (1 - 2 * (this.x * this.x + this.y * this.y)));
            y = Math.Atan(2 * (this.w * this.z + this.x * this.y) / (1 - 2 * (this.y * this.y + this.z * this.z)));
            z = Math.Asin(2 * (this.w * this.y - this.x * this.z));

            return new Vecteur3F(x, y, z);
        }

        public static Vecteur3F operator *(Quaternion A, Vecteur3F B)
        {
            double t0, t1, t2;
            double x, y, z;

            t0 = -A.x * B.X - A.y * B.Y - A.z * B.Z;
            t1 = A.w * B.X + A.y * B.Z - A.z * B.Y;
            t2 = A.w * B.Y + A.z * B.X - A.x * B.Z;

            z = A.w * B.Z + A.x * B.Y - A.y * B.X;
            x = t1;
            y = t2;

            t1 = t0 * -A.x + x * A.w - y * A.z + z * A.y;
            t2 = t0 * -A.y + y * A.w - z * A.x + x * A.z;

            z = t0 * -A.z + z * A.w - x * A.y + y * A.x;
            x = t1;
            y = t2;

            return new Vecteur3F(x, y, z);
        }

        public static Quaternion operator -(Quaternion A)
        {
            double d = A.x * A.x + A.y * A.y + A.z * A.z + A.w * A.w;
            return new Quaternion(-A.x / d, -A.y / d, -A.z / d, A.w / d);
        }

        public static Quaternion operator *(Quaternion A, Quaternion B)
        {
            double x, y, z, w;

            w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z;
            x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y;
            y = A.w * B.y + A.y * B.w + A.z * B.x - A.x * B.z;
            z = A.w * B.z + A.z * B.w + A.x * B.y - A.y * B.x;

            return new Quaternion(x, y, z, w);
        }

        /* PROPERTIES */

        public double Angle
        {
            get
            {
                return Math.Acos(this.w) * 2;
            }
        }

        public double X
        {
            get
            {
                if (this.w == 1)
                {
                    return 1;
                }
                else
                {
                    return this.x / Math.Sqrt(1 - this.w * this.w);
                }
            }
        }

        public double Y
        {
            get
            {
                if (this.w == 1)
                {
                    return 0;
                }
                else
                {
                    return this.y / Math.Sqrt(1 - this.w * this.w);
                }
            }
        }

        public double Z
        {
            get
            {
                if (this.w == 1)
                {
                    return 0;
                }
                else
                {
                    return this.z / Math.Sqrt(1 - this.w * this.w);
                }
            }
        }
    }
}
