class Quaternion
    {
        public double X, Y, Z, W;
        //Quaternion Initialization
        public Quaternion(double w, double x, double y, double z)
        {
            W = w;
            X = x;
            Y = y;
            Z = z;
        }

        //Use Axis and Angle to get a Quaternion
        public Quaternion(double [] Axis, double Angle)
        {

        }

        //Normalise
        public void Normalise()
        {
            double m = W * W + X * X + Y * Y + Z * Z;
            if (m > 0.001)
            {
                m = Math.Sqrt(m);
                W /= m;
                X /= m;
                Y /= m;
                Z /= m;
            }
            else
            {
                W = 1; X = 0; Y = 0; Z = 0;
            }
        }

        //Quaternion Conjugate
        public static Quaternion Conjugate(Quaternion q)
        {
            double nw = q.W;
            double nx = -(q.X);
            double ny = -(q.Y);
            double nz = -(q.Z);
            return new Quaternion(nw, nx, ny, nz);
        }

        //Quaternion Addition
        public static Quaternion operator +(Quaternion q1, Quaternion q2)
        {
            double nw = q1.W + q2.W;
            double nx = q1.X + q2.X;
            double ny = q1.Y + q2.Y;
            double nz = q1.Z + q2.Z;
            return new Quaternion(nw, nx, ny, nz);
        }

        //Quaternion Substration
        public static Quaternion operator -(Quaternion q1, Quaternion q2)
        {
            double nw = q1.W - q2.W;
            double nx = q1.X - q2.X;
            double ny = q1.Y - q2.Y;
            double nz = q1.Z - q2.Z;
            return new Quaternion(nw, nx, ny, nz);
        }

        //Quaternion Multiplication
        // Multiplying q1 with q2 is meaning of doing q2 firstly then q1
        public static Quaternion operator *(Quaternion q1, Quaternion q2)
        {
            double nw = q1.W * q2.W - q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z;
            double nx = q1.W * q2.X + q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y;
            double ny = q1.W * q2.Y - q1.X * q2.Z + q1.Y * q2.W + q1.Z * q2.X;
            double nz = q1.W * q2.Z + q1.X * q2.Y - q1.Y * q2.X + q1.Z * q2.W;
            return new Quaternion(nw, nx, ny, nz);
        }

        //Quaternion Division (Multiply by the inverse)
        public static Quaternion operator /(Quaternion q1, Quaternion q2)
        {
            Quaternion q2Conj = Conjugate(q2);
            double norm = Math.Sqrt(q2.W * q2.W + q2.X * q2.X + q2.Y * q2.Y + q2.Z * q2.Z);
            Quaternion newQ = q1 * q2Conj;
            double nw = newQ.W / norm;
            double nx = newQ.X / norm;
            double ny = newQ.Y / norm;
            double nz = newQ.Z / norm;
            return new Quaternion(nw, nx, ny, nz);
        }

        //Get the angle of rotation
        public void GetAngle()
        {

        }

        //Do the slerp
        public static Quaternion Slerp(Quaternion q1, Quaternion q2, double t)
        {
            //w, x, y, z
            double nw, nx, ny, nz;
            //Calculate angle between them
            double cosHalfTheta = q1.W * q2.W + q1.X * q2.X + q1.Y + q2.Y + q1.Z + q1.Z;


            //if q1 = q2 or q1 = -q2 then theta = 0, return q1
            if (Math.Abs(cosHalfTheta) >= 1.0)
            {
                nw = q2.W;
                nx = q2.X;
                ny = q2.Y;
                nz = q2.Z;
                return new Quaternion(nw, nx, ny, nz);
            }

            //Calculate temporary values.
            double halfTheta = Math.Acos(cosHalfTheta);
            double sinHalfTheta = Math.Sqrt(1.0 - cosHalfTheta * cosHalfTheta);

            //if theta = 180 degrees then result is not fully defined
            if(Math.Abs(sinHalfTheta) < 0.001){
                nw = (q1.W * 0.5 + q2.W * 0.5);
                nx = (q1.X * 0.5 + q2.X * 0.5);
                ny = (q1.Y * 0.5 + q2.Y * 0.5);
                nz = (q1.Z * 0.5 + q2.Z * 0.5);
                return new Quaternion(nw, nx, ny, nz);
            }

            double ratioA = Math.Sin((1 - t) * halfTheta) / sinHalfTheta;
            double ratioB = Math.Sin(t * halfTheta) / sinHalfTheta;
            //Calculate Quaternion
            nw = (q1.W * ratioA + q2.W * ratioB);
            nx = (q1.X * ratioA + q2.X * ratioB);
            ny = (q1.Y * ratioA + q2.Y * ratioB);
            nz = (q1.Z * ratioA + q2.Z * ratioB);
            return new Quaternion(nw, nx, ny, nz);
        }

    }