using System;
#if UNITY_5_3_OR_NEWER
using Quaternion = UnityEngine.Quaternion;
#endif
namespace Passer.LinearAlgebra {

    public class QuaternionOf<T> {
        public T x;
        public T y;
        public T z;
        public T w;

        public QuaternionOf(T x, T y, T z, T w) {
            this.x = x;
            this.y = y;
            this.z = z;
            this.w = w;
        }

#if UNITY_5_3_OR_NEWER
        public static Matrix2 ToRotationMatrix(Quaternion q) {
            float w = q.x, x = q.y, y = q.z, z = q.w;

            float[,] result = new float[,]
            {
                { 1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y) },
                { 2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x) },
                { 2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y) }
            };
            return new Matrix2(result);
        }

        public static Quaternion FromRotationMatrix(Matrix2 m) {
            float trace = m.data[0, 0] + m.data[1, 1] + m.data[2, 2];
            float w, x, y, z;

            if (trace > 0) {
                float s = 0.5f / (float)Math.Sqrt(trace + 1.0f);
                w = 0.25f / s;
                x = (m.data[2, 1] - m.data[1, 2]) * s;
                y = (m.data[0, 2] - m.data[2, 0]) * s;
                z = (m.data[1, 0] - m.data[0, 1]) * s;
            }
            else {
                if (m.data[0, 0] > m.data[1, 1] && m.data[0, 0] > m.data[2, 2]) {
                    float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[0, 0] - m.data[1, 1] - m.data[2, 2]);
                    w = (m.data[2, 1] - m.data[1, 2]) / s;
                    x = 0.25f * s;
                    y = (m.data[0, 1] + m.data[1, 0]) / s;
                    z = (m.data[0, 2] + m.data[2, 0]) / s;
                }
                else if (m.data[1, 1] > m.data[2, 2]) {
                    float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[1, 1] - m.data[0, 0] - m.data[2, 2]);
                    w = (m.data[0, 2] - m.data[2, 0]) / s;
                    x = (m.data[0, 1] + m.data[1, 0]) / s;
                    y = 0.25f * s;
                    z = (m.data[1, 2] + m.data[2, 1]) / s;
                }
                else {
                    float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[2, 2] - m.data[0, 0] - m.data[1, 1]);
                    w = (m.data[1, 0] - m.data[0, 1]) / s;
                    x = (m.data[0, 2] + m.data[2, 0]) / s;
                    y = (m.data[1, 2] + m.data[2, 1]) / s;
                    z = 0.25f * s;
                }
            }

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

    // public class Quaternion : QuaternionOf<float> {
    //     public Quaternion(float x, float y, float z, float w) : base(x, y, z, w) { }
    // }
}