using System;
using Microsoft.Xna.Framework;

namespace Utility
{
    class QuaternalSmoother
    {
        #region Variables

        Quaternion quat;
        Vector2 actual;
        Vector2 target;
        Vector2 tester;
        float length;
        float angle;
        float thresh;
        float spring;

        #endregion

        #region Accessors

        public Vector2 Vector
        {
            get { return actual; }
        }

        public Vector2 Target
        {
            set
            { tester = value; }
        }

        public float Angle
        {
            get { return angle; }
        }

        public float Length
        {
            get { return length; }
        }

        #endregion

        #region System

        public QuaternalSmoother(Vector2 vec, float spring, float threshold)
        {
            this.actual = vec;
            this.target = vec;
            this.tester = vec;
            this.spring = spring;
            this.thresh = threshold;
            this.length = vec.Length();

            actual.Normalize();

            angle = (float)Math.Acos(Vector2.Dot(Vector2.UnitY, actual ) );
            if (target.Y < 0.0f) angle = -angle;

            Vector3 axis = Vector3.UnitZ;
            Quaternion.CreateFromAxisAngle( ref axis, angle, out quat );
        }
        public void Update(GameTime gameTime)
        {
            length = tester.Length();
            if (length > thresh)
            {
                target = tester;
            }

            target.Normalize();
            float temp = (float)Math.Acos(Vector2.Dot(Vector2.UnitX, target ) );
            if ( target.Y > 0f ) temp = -temp;

            Quaternion test;
            Vector3 axis = Vector3.UnitZ;
            Quaternion.CreateFromAxisAngle( ref axis, temp, out test );
            quat = Quaternion.Lerp( quat, test, spring );

            Vector3 vec = QuaternionToEuler( quat );
            angle = vec.Z;

            actual.X = (float)Math.Cos( angle );
            actual.Y = (float)Math.Sin( angle );
            actual.Normalize();
        }

        #endregion

        #region Methods

        private Vector3 QuaternionToEuler(Quaternion rotation)
        {
            float q0 = rotation.W;
            float q1 = rotation.Y;
            float q2 = rotation.X;
            float q3 = rotation.Z;

            //In Radians
            Vector3 angles = new Vector3();
            angles.X = (float)Math.Atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (Math.Pow(q1, 2) + Math.Pow(q2, 2)));
            angles.Y = (float)Math.Asin(2 * (q0 * q2 - q3 * q1));
            angles.Z = (float)Math.Atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (Math.Pow(q2, 2) + Math.Pow(q3, 2)));

            return angles;
        }

        #endregion

    }
}