using System.Runtime.InteropServices;
using UnityEngine;

namespace Exoa.Utils
{
    [System.Serializable]
    public class Springs
    {
        public enum ParameterMode
        {
            Exponential,
            OscillationByHalfLife,
            OscillationByDampingRatio,
        };

        public ParameterMode type;
        [Range(0f, 5f)]
        public float halfLife = .1f;
        [Range(0f, 100f)]
        public float frequency = 8f;
        [Range(0f, 100f)]
        public float angularFrequency = 8f;
        [Range(0f, 1f)]
        public float ratio = 1f;

        public float UpdateSpring(ref FloatSpring spring, float target)
        {
            var result = target;
            switch (type)
            {
                case Springs.ParameterMode.Exponential:
                    result = spring.TrackExponential(target, halfLife, Time.deltaTime);
                    break;
                case Springs.ParameterMode.OscillationByHalfLife:
                    result = spring.TrackHalfLife(target, frequency, halfLife, Time.deltaTime);
                    break;
                case Springs.ParameterMode.OscillationByDampingRatio:
                    result = spring.TrackDampingRatio(target, angularFrequency, ratio, Time.deltaTime);
                    break;
            }
            return result;
        }

        public Vector2 UpdateSpring(ref Vector2Spring spring, Vector2 target)
        {

            var result = target;
            switch (type)
            {
                case Springs.ParameterMode.Exponential:
                    result = spring.TrackExponential(target, halfLife, Time.deltaTime);
                    break;
                case Springs.ParameterMode.OscillationByHalfLife:
                    result = spring.TrackHalfLife(target, frequency, halfLife, Time.deltaTime);
                    break;
                case Springs.ParameterMode.OscillationByDampingRatio:
                    result = spring.TrackDampingRatio(target, angularFrequency, ratio, Time.deltaTime);
                    break;
            }
            //Debug.Log("UpdateSpring target:" + target + " result:" + result + " dt:" + Time.deltaTime);
            return result;
        }

        public Vector3 UpdateSpring(ref Vector3Spring spring, Vector3 target)
        {
            var result = target;
            switch (type)
            {
                case Springs.ParameterMode.Exponential:
                    result = spring.TrackExponential(target, halfLife, Time.deltaTime);
                    break;
                case Springs.ParameterMode.OscillationByHalfLife:
                    result = spring.TrackHalfLife(target, frequency, halfLife, Time.deltaTime);
                    break;
                case Springs.ParameterMode.OscillationByDampingRatio:
                    result = spring.TrackDampingRatio(target, angularFrequency, ratio, Time.deltaTime);
                    break;
            }
            return result;
        }

        public Vector4 UpdateSpring(ref Vector4Spring spring, Vector4 target)
        {
            var result = target;
            switch (type)
            {
                case Springs.ParameterMode.Exponential:
                    result = spring.TrackExponential(target, halfLife, Time.deltaTime);
                    break;
                case Springs.ParameterMode.OscillationByHalfLife:
                    result = spring.TrackHalfLife(target, frequency, halfLife, Time.deltaTime);
                    break;
                case Springs.ParameterMode.OscillationByDampingRatio:
                    result = spring.TrackDampingRatio(target, angularFrequency, ratio, Time.deltaTime);
                    break;
            }
            return result;
        }

        public Quaternion UpdateSpring(ref QuaternionSpring spring, Quaternion target)
        {
            var result = target;
            switch (type)
            {
                case Springs.ParameterMode.Exponential:
                    result = spring.TrackExponential(target, halfLife, Time.deltaTime);
                    break;
                case Springs.ParameterMode.OscillationByHalfLife:
                    result = spring.TrackHalfLife(target, frequency, halfLife, Time.deltaTime);
                    break;
                case Springs.ParameterMode.OscillationByDampingRatio:
                    result = spring.TrackDampingRatio(target, angularFrequency, ratio, Time.deltaTime);
                    break;
            }
            return result;
        }
    }

    public static class ColorExt
    {
        public static Color ToColor(this Vector4 v)
        {
            return new Color(v.x, v.y, v.z, v.w);
        }
        public static Vector4 ToVector4(this Color v)
        {
            return new Vector4(v.r, v.g, v.b, v.a);
        }
    }



    [StructLayout(LayoutKind.Sequential, Pack = 0)]
    public struct FloatSpring
    {
        public static readonly int Stride = 2 * sizeof(float);

        public float Value;
        public float Velocity;

        public void Reset()
        {
            Value = 0.0f;
            Velocity = 0.0f;
        }

        public void Reset(float initValue)
        {
            Value = initValue;
            Velocity = 0.0f;
        }

        public void Reset(float initValue, float initVelocity)
        {
            Value = initValue;
            Velocity = initVelocity;
        }

        public float TrackDampingRatio(float targetValue, float angularFrequency, float dampingRatio, float deltaTime)
        {
            if (angularFrequency < MathUtil.Epsilon)
            {
                Velocity = 0.0f;
                return Value;
            }

            float delta = targetValue - Value;

            float f = 1.0f + 2.0f * deltaTime * dampingRatio * angularFrequency;
            float oo = angularFrequency * angularFrequency;
            float hoo = deltaTime * oo;
            float hhoo = deltaTime * hoo;
            float detInv = 1.0f / (f + hhoo);
            float detX = f * Value + deltaTime * Velocity + hhoo * targetValue;
            float detV = Velocity + hoo * delta;

            Velocity = detV * detInv;
            Value = detX * detInv;

            if (Velocity < MathUtil.Epsilon && delta < MathUtil.Epsilon)
            {
                Velocity = 0.0f;
                Value = targetValue;
            }

            return Value;
        }

        public float TrackHalfLife(float targetValue, float frequencyHz, float halfLife, float deltaTime)
        {
            if (halfLife < MathUtil.Epsilon)
            {
                Velocity = 0.0f;
                Value = targetValue;
                return Value;
            }

            float angularFrequency = frequencyHz * MathUtil.TwoPi;
            float dampingRatio = 0.6931472f / (angularFrequency * halfLife);
            return TrackDampingRatio(targetValue, angularFrequency, dampingRatio, deltaTime);
        }

        public float TrackExponential(float targetValue, float halfLife, float deltaTime)
        {
            if (halfLife < MathUtil.Epsilon)
            {
                Velocity = 0.0f;
                Value = targetValue;
                return Value;
            }

            float angularFrequency = 0.6931472f / halfLife;
            float dampingRatio = 1.0f;
            return TrackDampingRatio(targetValue, angularFrequency, dampingRatio, deltaTime);
        }
    }


    [StructLayout(LayoutKind.Sequential, Pack = 0)]
    public struct Vector2Spring
    {
        public static readonly int Stride = 4 * sizeof(float);

        public Vector2 Value;
        public Vector2 Velocity;

        public void Reset()
        {
            Value = Vector2.zero;
            Velocity = Vector2.zero;
        }

        public void Reset(Vector2 initValue)
        {
            Value = initValue;
            Velocity = Vector2.zero;
        }

        public void Reset(Vector2 initValue, Vector2 initVelocity)
        {
            Value = initValue;
            Velocity = initVelocity;
        }

        public Vector2 TrackDampingRatio(Vector2 targetValue, float angularFrequency, float dampingRatio, float deltaTime)
        {
            if (angularFrequency < MathUtil.Epsilon)
            {
                Velocity = Vector2.zero;
                return Value;
            }

            Vector2 delta = targetValue - Value;

            float f = 1.0f + 2.0f * deltaTime * dampingRatio * angularFrequency;
            float oo = angularFrequency * angularFrequency;
            float hoo = deltaTime * oo;
            float hhoo = deltaTime * hoo;
            float detInv = 1.0f / (f + hhoo);
            Vector2 detX = f * Value + deltaTime * Velocity + hhoo * targetValue;
            Vector2 detV = Velocity + hoo * delta;

            Velocity = detV * detInv;
            Value = detX * detInv;

            if (Velocity.magnitude < MathUtil.Epsilon && delta.magnitude < MathUtil.Epsilon)
            {
                Velocity = Vector2.zero;
                Value = targetValue;
            }

            return Value;
        }

        public Vector2 TrackHalfLife(Vector2 targetValue, float frequencyHz, float halfLife, float deltaTime)
        {
            if (halfLife < MathUtil.Epsilon)
            {
                Velocity = Vector2.zero;
                Value = targetValue;
                return Value;
            }

            float angularFrequency = frequencyHz * MathUtil.TwoPi;
            float dampingRatio = 0.6931472f / (angularFrequency * halfLife);
            return TrackDampingRatio(targetValue, angularFrequency, dampingRatio, deltaTime);
        }

        public Vector2 TrackExponential(Vector2 targetValue, float halfLife, float deltaTime)
        {
            if (halfLife < MathUtil.Epsilon)
            {
                Velocity = Vector2.zero;
                Value = targetValue;
                return Value;
            }

            float angularFrequency = 0.6931472f / halfLife;
            float dampingRatio = 1.0f;
            return TrackDampingRatio(targetValue, angularFrequency, dampingRatio, deltaTime);
        }
    }


    [StructLayout(LayoutKind.Sequential, Pack = 0)]
    public struct Vector3Spring
    {
        public static readonly int Stride = 8 * sizeof(float);

        public Vector3 Value;
        private float m_padding0;
        public Vector3 Velocity;
        private float m_padding1;

        public void Reset()
        {
            Value = Vector3.zero;
            Velocity = Vector3.zero;
        }

        public void Reset(Vector3 initValue)
        {
            Value = initValue;
            Velocity = Vector3.zero;
        }

        public void Reset(Vector3 initValue, Vector3 initVelocity)
        {
            Value = initValue;
            Velocity = initVelocity;
        }

        public Vector3 TrackDampingRatio(Vector3 targetValue, float angularFrequency, float dampingRatio, float deltaTime)
        {
            if (angularFrequency < MathUtil.Epsilon)
            {
                Velocity = Vector3.zero;
                return Value;
            }

            Vector3 delta = targetValue - Value;

            float f = 1.0f + 2.0f * deltaTime * dampingRatio * angularFrequency;
            float oo = angularFrequency * angularFrequency;
            float hoo = deltaTime * oo;
            float hhoo = deltaTime * hoo;
            float detInv = 1.0f / (f + hhoo);
            Vector3 detX = f * Value + deltaTime * Velocity + hhoo * targetValue;
            Vector3 detV = Velocity + hoo * delta;

            Velocity = detV * detInv;
            Value = detX * detInv;

            if (Velocity.magnitude < MathUtil.Epsilon && delta.magnitude < MathUtil.Epsilon)
            {
                Velocity = Vector3.zero;
                Value = targetValue;
            }

            return Value;
        }

        public Vector3 TrackHalfLife(Vector3 targetValue, float frequencyHz, float halfLife, float deltaTime)
        {
            if (halfLife < MathUtil.Epsilon)
            {
                Velocity = Vector3.zero;
                Value = targetValue;
                return Value;
            }

            float angularFrequency = frequencyHz * MathUtil.TwoPi;
            float dampingRatio = 0.6931472f / (angularFrequency * halfLife);
            return TrackDampingRatio(targetValue, angularFrequency, dampingRatio, deltaTime);
        }

        public Vector3 TrackExponential(Vector3 targetValue, float halfLife, float deltaTime)
        {
            if (halfLife < MathUtil.Epsilon)
            {
                Velocity = Vector3.zero;
                Value = targetValue;
                return Value;
            }

            float angularFrequency = 0.6931472f / halfLife;
            float dampingRatio = 1.0f;
            return TrackDampingRatio(targetValue, angularFrequency, dampingRatio, deltaTime);
        }
    }


    [StructLayout(LayoutKind.Sequential, Pack = 0)]
    public struct Vector4Spring
    {
        public static readonly int Stride = 8 * sizeof(float);

        public Vector4 Value;
        public Vector4 Velocity;

        public void Reset()
        {
            Value = Vector4.zero;
            Velocity = Vector4.zero;
        }

        public void Reset(Vector4 initValue)
        {
            Value = initValue;
            Velocity = Vector4.zero;
        }

        public void Reset(Vector4 initValue, Vector4 initVelocity)
        {
            Value = initValue;
            Velocity = initVelocity;
        }

        public Vector4 TrackDampingRatio(Vector4 targetValue, float angularFrequency, float dampingRatio, float deltaTime)
        {
            if (angularFrequency < MathUtil.Epsilon)
            {
                Velocity = Vector4.zero;
                return Value;
            }

            Vector4 delta = targetValue - Value;

            float f = 1.0f + 2.0f * deltaTime * dampingRatio * angularFrequency;
            float oo = angularFrequency * angularFrequency;
            float hoo = deltaTime * oo;
            float hhoo = deltaTime * hoo;
            float detInv = 1.0f / (f + hhoo);
            Vector4 detX = f * Value + deltaTime * Velocity + hhoo * targetValue;
            Vector4 detV = Velocity + hoo * delta;

            Velocity = detV * detInv;
            Value = detX * detInv;

            if (Velocity.magnitude < MathUtil.Epsilon && delta.magnitude < MathUtil.Epsilon)
            {
                Velocity = Vector4.zero;
                Value = targetValue;
            }

            return Value;
        }

        public Vector4 TrackHalfLife(Vector4 targetValue, float frequencyHz, float halfLife, float deltaTime)
        {
            if (halfLife < MathUtil.Epsilon)
            {
                Velocity = Vector4.zero;
                Value = targetValue;
                return Value;
            }

            float angularFrequency = frequencyHz * MathUtil.TwoPi;
            float dampingRatio = 0.6931472f / (angularFrequency * halfLife);
            return TrackDampingRatio(targetValue, angularFrequency, dampingRatio, deltaTime);
        }

        public Vector4 TrackExponential(Vector4 targetValue, float halfLife, float deltaTime)
        {
            if (halfLife < MathUtil.Epsilon)
            {
                Velocity = Vector4.zero;
                Value = targetValue;
                return Value;
            }

            float angularFrequency = 0.6931472f / halfLife;
            float dampingRatio = 1.0f;
            return TrackDampingRatio(targetValue, angularFrequency, dampingRatio, deltaTime);
        }
    }



    [StructLayout(LayoutKind.Sequential, Pack = 0)]
    public struct QuaternionSpring
    {
        public static readonly int Stride = 8 * sizeof(float);

        public Vector4 ValueVec;
        public Vector4 VelocityVec;

        public Quaternion ValueQuat
        {
            get { return QuaternionUtil.FromVector4(ValueVec); }
            set { ValueVec = QuaternionUtil.ToVector4(value); }
        }

        public void Reset()
        {
            ValueVec = QuaternionUtil.ToVector4(Quaternion.identity);
            VelocityVec = Vector4.zero;
        }

        public void Reset(Vector4 initValue)
        {
            ValueVec = initValue;
            VelocityVec = Vector4.zero;
        }

        public void Reset(Vector4 initValue, Vector4 initVelocity)
        {
            ValueVec = initValue;
            VelocityVec = initVelocity;
        }

        public void Reset(Quaternion initValue)
        {
            ValueVec = QuaternionUtil.ToVector4(initValue);
            VelocityVec = Vector4.zero;
        }

        public void Reset(Quaternion initValue, Quaternion initVelocity)
        {
            ValueVec = QuaternionUtil.ToVector4(initValue);
            VelocityVec = QuaternionUtil.ToVector4(initVelocity);
        }

        public Quaternion TrackDampingRatio(Vector4 targetValueVec, float angularFrequency, float dampingRatio, float deltaTime)
        {
            if (angularFrequency < MathUtil.Epsilon)
            {
                VelocityVec = QuaternionUtil.ToVector4(Quaternion.identity);
                return QuaternionUtil.FromVector4(ValueVec);
            }

            // keep in same hemisphere for shorter track delta
            if (Vector4.Dot(ValueVec, targetValueVec) < 0.0f)
            {
                targetValueVec = -targetValueVec;
            }

            Vector4 delta = targetValueVec - ValueVec;

            float f = 1.0f + 2.0f * deltaTime * dampingRatio * angularFrequency;
            float oo = angularFrequency * angularFrequency;
            float hoo = deltaTime * oo;
            float hhoo = deltaTime * hoo;
            float detInv = 1.0f / (f + hhoo);
            Vector4 detX = f * ValueVec + deltaTime * VelocityVec + hhoo * targetValueVec;
            Vector4 detV = VelocityVec + hoo * delta;

            VelocityVec = detV * detInv;
            ValueVec = detX * detInv;

            if (VelocityVec.magnitude < MathUtil.Epsilon && delta.magnitude < MathUtil.Epsilon)
            {
                VelocityVec = Vector4.zero;
                ValueVec = targetValueVec;
            }

            return QuaternionUtil.FromVector4(ValueVec);
        }

        public Quaternion TrackDampingRatio(Quaternion targetValue, float angularFrequency, float dampingRatio, float deltaTime)
        {
            return TrackDampingRatio(QuaternionUtil.ToVector4(targetValue), angularFrequency, dampingRatio, deltaTime);
        }

        public Quaternion TrackHalfLife(Vector4 targetValueVec, float frequencyHz, float halfLife, float deltaTime)
        {
            if (halfLife < MathUtil.Epsilon)
            {
                VelocityVec = Vector4.zero;
                ValueVec = targetValueVec;
                return QuaternionUtil.FromVector4(targetValueVec);
            }

            float angularFrequency = frequencyHz * MathUtil.TwoPi;
            float dampingRatio = 0.6931472f / (angularFrequency * halfLife);
            return TrackDampingRatio(targetValueVec, angularFrequency, dampingRatio, deltaTime);
        }

        public Quaternion TrackHalfLife(Quaternion targetValue, float frequencyHz, float halfLife, float deltaTime)
        {
            if (halfLife < MathUtil.Epsilon)
            {
                VelocityVec = QuaternionUtil.ToVector4(Quaternion.identity);
                ValueVec = QuaternionUtil.ToVector4(targetValue);
                return targetValue;
            }

            float angularFrequency = frequencyHz * MathUtil.TwoPi;
            float dampingRatio = 0.6931472f / (angularFrequency * halfLife);
            return TrackDampingRatio(targetValue, angularFrequency, dampingRatio, deltaTime);
        }

        public Quaternion TrackExponential(Vector4 targetValueVec, float halfLife, float deltaTime)
        {
            if (halfLife < MathUtil.Epsilon)
            {
                VelocityVec = Vector4.zero;
                ValueVec = targetValueVec;
                return QuaternionUtil.FromVector4(targetValueVec);
            }

            float angularFrequency = 0.6931472f / halfLife;
            float dampingRatio = 1.0f;
            return TrackDampingRatio(targetValueVec, angularFrequency, dampingRatio, deltaTime);
        }

        public Quaternion TrackExponential(Quaternion targetValue, float halfLife, float deltaTime)
        {
            if (halfLife < MathUtil.Epsilon)
            {
                VelocityVec = QuaternionUtil.ToVector4(Quaternion.identity);
                ValueVec = QuaternionUtil.ToVector4(targetValue);
                return targetValue;
            }

            float angularFrequency = 0.6931472f / halfLife;
            float dampingRatio = 1.0f;
            return TrackDampingRatio(targetValue, angularFrequency, dampingRatio, deltaTime);
        }
    }


    public class MathUtil
    {
        public static readonly float Pi = Mathf.PI;
        public static readonly float TwoPi = 2.0f * Mathf.PI;
        public static readonly float HalfPi = Mathf.PI / 2.0f;
        public static readonly float QuaterPi = Mathf.PI / 4.0f;
        public static readonly float SixthPi = Mathf.PI / 6.0f;

        public static readonly float Sqrt2 = Mathf.Sqrt(2.0f);
        public static readonly float Sqrt2Inv = 1.0f / Mathf.Sqrt(2.0f);
        public static readonly float Sqrt3 = Mathf.Sqrt(3.0f);
        public static readonly float Sqrt3Inv = 1.0f / Mathf.Sqrt(3.0f);

        public static readonly float Epsilon = 1.0e-6f;
        public static readonly float Rad2Deg = 180.0f / Mathf.PI;
        public static readonly float Deg2Rad = Mathf.PI / 180.0f;

        public static float AsinSafe(float x)
        {
            return Mathf.Asin(Mathf.Clamp(x, -1.0f, 1.0f));
        }

        public static float AcosSafe(float x)
        {
            return Mathf.Acos(Mathf.Clamp(x, -1.0f, 1.0f));
        }

        public static float InvSafe(float x)
        {
            return 1.0f / Mathf.Max(Epsilon, x);
        }

        public static float PointLineDist(Vector2 point, Vector2 linePos, Vector2 lineDir)
        {
            var delta = point - linePos;
            return (delta - Vector2.Dot(delta, lineDir) * lineDir).magnitude;
        }

        public static float PointSegmentDist(Vector2 point, Vector2 segmentPosA, Vector2 segmentPosB)
        {
            var segmentVec = segmentPosB - segmentPosA;
            float segmentDistInv = 1.0f / segmentVec.magnitude;
            var segmentDir = segmentVec * segmentDistInv;
            var delta = point - segmentPosA;
            float t = Vector2.Dot(delta, segmentDir) * segmentDistInv;
            var closest = segmentPosA + Mathf.Clamp(t, 0.0f, 1.0f) * segmentVec;
            return (closest - point).magnitude;
        }

        public static float Seek(float current, float target, float maxDelta)
        {
            float delta = target - current;
            delta = Mathf.Sign(delta) * Mathf.Min(maxDelta, Mathf.Abs(delta));
            return current + delta;
        }

        public static Vector2 Seek(Vector2 current, Vector2 target, float maxDelta)
        {
            Vector2 delta = target - current;
            float deltaMag = delta.magnitude;
            if (deltaMag < Epsilon)
                return target;

            delta = Mathf.Min(maxDelta, deltaMag) * delta.normalized;
            return current + delta;
        }

        public static float Remainder(float a, float b)
        {
            return a - (a / b) * b;
        }

        public static int Remainder(int a, int b)
        {
            return a - (a / b) * b;
        }

        public static float Modulo(float a, float b)
        {
            return Mathf.Repeat(a, b);
        }

        public static int Modulo(int a, int b)
        {
            int r = a % b;
            return r >= 0 ? r : r + b;
        }
    }

    public class QuaternionUtil
    {
        // basic stuff
        // ------------------------------------------------------------------------

        public static float Magnitude(Quaternion q)
        {
            return Mathf.Sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
        }

        public static float MagnitudeSqr(Quaternion q)
        {
            return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
        }

        public static Quaternion Normalize(Quaternion q)
        {
            float magInv = 1.0f / Magnitude(q);
            return new Quaternion(magInv * q.x, magInv * q.y, magInv * q.z, magInv * q.w);
        }

        // axis must be normalized
        public static Quaternion AxisAngle(Vector3 axis, float angle)
        {
            float h = 0.5f * angle;
            float s = Mathf.Sin(h);
            float c = Mathf.Cos(h);

            return new Quaternion(s * axis.x, s * axis.y, s * axis.z, c);
        }

        public static Vector3 GetAxis(Quaternion q)
        {
            Vector3 v = new Vector3(q.x, q.y, q.z);
            float len = v.magnitude;
            if (len < MathUtil.Epsilon)
                return Vector3.left;

            return v / len;
        }

        public static float GetAngle(Quaternion q)
        {
            return 2.0f * Mathf.Acos(Mathf.Clamp(q.w, -1.0f, 1.0f));
        }

        public static Quaternion FromAngularVector(Vector3 v)
        {
            float len = v.magnitude;
            if (len < MathUtil.Epsilon)
                return Quaternion.identity;

            v /= len;

            float h = 0.5f * len;
            float s = Mathf.Sin(h);
            float c = Mathf.Cos(h);

            return new Quaternion(s * v.x, s * v.y, s * v.z, c);
        }

        public static Vector3 ToAngularVector(Quaternion q)
        {
            Vector3 axis = GetAxis(q);
            float angle = GetAngle(q);

            return angle * axis;
        }

        public static Quaternion Pow(Quaternion q, float exp)
        {
            Vector3 axis = GetAxis(q);
            float angle = GetAngle(q) * exp;
            return AxisAngle(axis, angle);
        }

        // v: derivative of q
        public static Quaternion Integrate(Quaternion q, Quaternion v, float dt)
        {
            return Pow(v, dt) * q;
        }

        // omega: angular velocity (direction is axis, magnitude is angle)
        // https://www.ashwinnarayan.com/post/how-to-integrate-quaternions/
        // https://gafferongames.com/post/physics_in_3d/
        public static Quaternion Integrate(Quaternion q, Vector3 omega, float dt)
        {
            omega *= 0.5f;
            Quaternion p = (new Quaternion(omega.x, omega.y, omega.z, 0.0f)) * q;
            return Normalize(new Quaternion(q.x + p.x * dt, q.y + p.y * dt, q.z + p.z * dt, q.w + p.w * dt));
        }

        public static Vector4 ToVector4(Quaternion q)
        {
            return new Vector4(q.x, q.y, q.z, q.w);
        }

        public static Quaternion FromVector4(Vector4 v, bool normalize = true)
        {
            if (normalize)
            {
                float magSqr = v.sqrMagnitude;
                if (magSqr < MathUtil.Epsilon)
                    return Quaternion.identity;

                v /= Mathf.Sqrt(magSqr);
            }

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

        // ------------------------------------------------------------------------
        // end: basic stuff


        // swing-twist decomposition & interpolation
        // ------------------------------------------------------------------------

        public static void DecomposeSwingTwist
        (
          Quaternion q,
          Vector3 twistAxis,
          out Quaternion swing,
          out Quaternion twist
        )
        {
            Vector3 r = new Vector3(q.x, q.y, q.z); // (rotaiton axis) * cos(angle / 2)

            // singularity: rotation by 180 degree
            if (r.sqrMagnitude < MathUtil.Epsilon)
            {
                Vector3 rotatedTwistAxis = q * twistAxis;
                Vector3 swingAxis = Vector3.Cross(twistAxis, rotatedTwistAxis);

                if (swingAxis.sqrMagnitude > MathUtil.Epsilon)
                {
                    float swingAngle = Vector3.Angle(twistAxis, rotatedTwistAxis);
                    swing = Quaternion.AngleAxis(swingAngle, swingAxis);
                }
                else
                {
                    // more singularity: rotation axis parallel to twist axis
                    swing = Quaternion.identity; // no swing
                }

                // always twist 180 degree on singularity
                twist = Quaternion.AngleAxis(180.0f, twistAxis);
                return;
            }

            // formula & proof: 
            // http://www.euclideanspace.com/maths/geometry/rotations/for/decomposition/
            Vector3 p = Vector3.Project(r, twistAxis);
            twist = new Quaternion(p.x, p.y, p.z, q.w);
            twist = Normalize(twist);
            swing = q * Quaternion.Inverse(twist);
        }

        public enum SterpMode
        {
            // non-constant angular velocity, faster
            // use if interpolating across small angles or constant angular velocity is not important
            Nlerp,

            // constant angular velocity, slower
            // use if interpolating across large angles and constant angular velocity is important
            Slerp,
        };

        // same swing & twist parameters
        public static Quaternion Sterp
        (
          Quaternion a,
          Quaternion b,
          Vector3 twistAxis,
          float t,
          SterpMode mode = SterpMode.Slerp
        )
        {
            Quaternion swing;
            Quaternion twist;
            return Sterp(a, b, twistAxis, t, out swing, out twist, mode);
        }

        // same swing & twist parameters with individual interpolated swing & twist outputs
        public static Quaternion Sterp
        (
          Quaternion a,
          Quaternion b,
          Vector3 twistAxis,
          float t,
          out Quaternion swing,
          out Quaternion twist,
          SterpMode mode = SterpMode.Slerp
        )
        {
            return Sterp(a, b, twistAxis, t, t, out swing, out twist, mode);
        }

        // different swing & twist parameters
        public static Quaternion Sterp
        (
          Quaternion a,
          Quaternion b,
          Vector3 twistAxis,
          float tSwing,
          float tTwist,
          SterpMode mode = SterpMode.Slerp
        )
        {
            Quaternion swing;
            Quaternion twist;
            return Sterp(a, b, twistAxis, tSwing, tTwist, out swing, out twist, mode);
        }

        // master sterp function
        public static Quaternion Sterp
        (
          Quaternion a,
          Quaternion b,
          Vector3 twistAxis,
          float tSwing,
          float tTwist,
          out Quaternion swing,
          out Quaternion twist,
          SterpMode mode
        )
        {
            Quaternion q = b * Quaternion.Inverse(a);
            Quaternion swingFull;
            Quaternion twistFull;
            QuaternionUtil.DecomposeSwingTwist(q, twistAxis, out swingFull, out twistFull);

            switch (mode)
            {
                default:
                case SterpMode.Nlerp:
                    swing = Quaternion.Lerp(Quaternion.identity, swingFull, tSwing);
                    twist = Quaternion.Lerp(Quaternion.identity, twistFull, tTwist);
                    break;
                case SterpMode.Slerp:
                    swing = Quaternion.Slerp(Quaternion.identity, swingFull, tSwing);
                    twist = Quaternion.Slerp(Quaternion.identity, twistFull, tTwist);
                    break;
            }

            return twist * swing;
        }

        // ------------------------------------------------------------------------
        // end: swing-twist decomposition & interpolation
    }
}