using System;

namespace Passer.LinearAlgebra
{
    public class Quat32
    {
        public float x;
        public float y;
        public float z;
        public float w;

        public Quat32()
        {
            this.x = 0;
            this.y = 0;
            this.z = 0;
            this.w = 1;
        }

        public Quat32(float x, float y, float z, float w)
        {
            this.x = x;
            this.y = y;
            this.z = z;
            this.w = w;
        }

        public static Quat32 FromSwingTwist(SwingTwist s)
        {
            Quat32 q32 = Quat32.Euler(-s.swing.vertical, s.swing.horizontal, s.twist);
            return q32;
        }

        public static Quat32 Euler(float yaw, float pitch, float roll)
        {
            float rollOver2 = roll * Angle.Deg2Rad * 0.5f;
            float sinRollOver2 = (float)Math.Sin((float)rollOver2);
            float cosRollOver2 = (float)Math.Cos((float)rollOver2);
            float pitchOver2 = pitch * 0.5f;
            float sinPitchOver2 = (float)Math.Sin((float)pitchOver2);
            float cosPitchOver2 = (float)Math.Cos((float)pitchOver2);
            float yawOver2 = yaw * 0.5f;
            float sinYawOver2 = (float)Math.Sin((float)yawOver2);
            float cosYawOver2 = (float)Math.Cos((float)yawOver2);
            Quat32 result = new Quat32()
            {
                w = cosYawOver2 * cosPitchOver2 * cosRollOver2 +
                       sinYawOver2 * sinPitchOver2 * sinRollOver2,
                x = sinYawOver2 * cosPitchOver2 * cosRollOver2 +
                       cosYawOver2 * sinPitchOver2 * sinRollOver2,
                y = cosYawOver2 * sinPitchOver2 * cosRollOver2 -
                       sinYawOver2 * cosPitchOver2 * sinRollOver2,
                z = cosYawOver2 * cosPitchOver2 * sinRollOver2 -
                       sinYawOver2 * sinPitchOver2 * cosRollOver2
            };
            return result;
        }

        public void ToAngles(out float right, out float up, out float forward)
        {
            float test = this.x * this.y + this.z * this.w;
            if (test > 0.499f)
            {  // singularity at north pole
                right = 0;
                up = 2 * (float)Math.Atan2(this.x, this.w) * Angle.Rad2Deg;
                forward = 90;
                return;
                //return Vector3(0, 2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, 90);
            }
            else if (test < -0.499f)
            {  // singularity at south pole
                right = 0;
                up = -2 * (float)Math.Atan2(this.x, this.w) * Angle.Rad2Deg;
                forward = -90;
                return;
                //return Vector3(0, -2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, -90);
            }
            else
            {
                float sqx = this.x * this.x;
                float sqy = this.y * this.y;
                float sqz = this.z * this.z;

                right = (float)Math.Atan2(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) * Angle.Rad2Deg;
                up = (float)Math.Atan2(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) * Angle.Rad2Deg;
                forward = (float)Math.Asin(2 * test) * Angle.Rad2Deg;
                return;
                // return Vector3(
                //     atan2f(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) *
                //         Rad2Deg,
                //     atan2f(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) *
                //         Rad2Deg,
                //     asinf(2 * test) * Angle.Rad2Deg);
            }
        }

    }
}