using UnityEngine;
using System.Collections;
using System;

public class NewBehaviourScript : MonoBehaviour
{
    public Transform t1, t2, t3;
    // Use this for initialization
    void Start ()
    {
        Do (new Vector3 (50, 0, 90));

        Do (new Vector3 (0, 50, 90));

        Do (new Vector3 (90, 0, 50));

        Do (new Vector3 (90, 50, 0));

        Do (new Vector3 (50, 90, 0));
        Do (new Vector3 (0, 90, 50));
    }

    void Do (Vector3 eulers)
    {
        Debug.Log ("Eulers: " + eulers);
        //ChangeQ(ToQ (eulers), (q)=>Debug.Log(q + ": " + FromQ(q)));
        //return;// (w0.5, z-0.4, x0.6, y0.4):
        Debug.Log (".Euler: " + Quaternion.Euler (eulers)); // (0.6, 0.4, -0.4, 0.5)
        Debug.Log ("ToQ: " + ToQ (eulers));              // (0.5, -0.4, 0.2, 0.7)
        Debug.Log ("FromQ: " + FromQ (ToQ (eulers)));       // (55.0, 100.0, -11.0)
        Debug.Log ("FromQ2: " + FromQ2 (ToQ (eulers)));      // (-55.5, -6.3, 71.0)
        Debug.Log (".Euler.eulerAngles: " + Quaternion.Euler (eulers).eulerAngles); // (80.0, 235.0, 169.0)
        Debug.Log ("FromQ2(,Euler): " + FromQ2 (Quaternion.Euler (eulers)));     // (65.8, 1.9, 99.8)
        Debug.Log ("ToQ.eulerAngles: " + ToQ (eulers).eulerAngles);              // (70.0, 286.9, 341.4)
        Debug.Log ("FromQ(,Euler): " + FromQ (Quaternion.Euler (eulers)));      // (-65.8, 76.0, 4.6)
        Debug.Log ("--");

    }

    void ChangeQ (Quaternion q, Action<Quaternion> f)
    {
        for (int i1=0; i1<=3; i1++) {
            for (int i2=0; i2<=3; i2++) {
                for (int i3=0; i3<=3; i3++) {
                    for (int i4=0; i4<=3; i4++) {
                        f (new Quaternion (q [i1], q [i2], q [i3], q [i4]));
                    }
                }
            }
        }
    }

    public static Quaternion ToQ (Vector3 v)
    {
        return ToQ (v.y, v.x, v.z);
    }

    public static Vector3 FromQ (Quaternion q2)
    {
        Quaternion q = new Quaternion (q2.w, q2.z, q2.x, q2.y);
        Vector3 pitchYawRoll;
        pitchYawRoll.y = (float)Math.Atan2 (2f * q.x * q.w + 2f * q.y * q.z, 1 - 2f * (q.z * q.z + q.w * q.w));     // Yaw
        pitchYawRoll.x = (float)Math.Asin (2f * (q.x * q.z - q.w * q.y));                             // Pitch
        pitchYawRoll.z = (float)Math.Atan2 (2f * q.x * q.y + 2f * q.z * q.w, 1 - 2f * (q.y * q.y + q.z * q.z));      // Roll
        return new Vector3 (pitchYawRoll.x * Mathf.Rad2Deg, pitchYawRoll.y * Mathf.Rad2Deg, pitchYawRoll.z * Mathf.Rad2Deg);
    }

    public static Quaternion ToQ (float yaw, float pitch, float roll)
    {
        yaw *= Mathf.Deg2Rad;
        pitch *= Mathf.Deg2Rad;
        roll *= Mathf.Deg2Rad;
        float rollOver2 = roll * 0.5f;
        float sinRollOver2 = (float)Math.Sin ((double)rollOver2);
        float cosRollOver2 = (float)Math.Cos ((double)rollOver2);
        float pitchOver2 = pitch * 0.5f;
        float sinPitchOver2 = (float)Math.Sin ((double)pitchOver2);
        float cosPitchOver2 = (float)Math.Cos ((double)pitchOver2);
        float yawOver2 = yaw * 0.5f;
        float sinYawOver2 = (float)Math.Sin ((double)yawOver2);
        float cosYawOver2 = (float)Math.Cos ((double)yawOver2);
        Quaternion result;
        result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
        result.x = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
        result.y = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
        result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;

        return result;
    }

    public static Vector3 FromQ2 (Quaternion q1)
    {
        float sqw = q1.w * q1.w;
        float sqx = q1.x * q1.x;
        float sqy = q1.y * q1.y;
        float sqz = q1.z * q1.z;
        float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
        float test = q1.x * q1.y + q1.z * q1.w;
        Vector3 v;
        Debug.Log ("u: " + unit + ", test: " + test + ", test2: " + (q1.x * q1.z + q1.y * q1.w)
            + ", test3: " + (q1.x * q1.w + q1.z * q1.y));
        if (test > 0.4999 * unit) { // singularity at north pole
            Debug.Log ("singularity1");
            v.y = 2 * Mathf.Atan2 (q1.x, q1.w);
            v.x = Mathf.PI / 2;
            v.z = 0;
            return v * Mathf.Rad2Deg;
        }
        if (test < -0.4999 * unit) { // singularity at south pole
            Debug.Log ("singularity2");
            v.y = -2 * Mathf.Atan2 (q1.x, q1.w);
            v.x = -Mathf.PI / 2;
            v.z = 0;
            return v * Mathf.Rad2Deg;
        }
        Quaternion q = new Quaternion (q1.w, q1.z, q1.x, q1.y);
        v.y = (float)Math.Atan2 (2f * q.x * q.w + 2f * q.y * q.z, 1 - 2f * (q.z * q.z + q.w * q.w));     // Yaw
        v.x = (float)Math.Asin (2f * (q.x * q.z - q.w * q.y));                             // Pitch
        v.z = (float)Math.Atan2 (2f * q.x * q.y + 2f * q.z * q.w, 1 - 2f * (q.y * q.y + q.z * q.z));      // Roll
        return v * Mathf.Rad2Deg;
    }

}