using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

namespace GraphiXNA.MathHelpers
{
    public sealed class Rotation
    {
        private static Vector3 UNIT_Y = Vector3.UnitY;
        private static Vector3 UNIT_X = Vector3.UnitX;
        private static Vector3 UNIT_Z = Vector3.UnitZ;

        internal Quaternion _Quaternion = Quaternion.Identity;

        internal float _Yaw = 0, _Pitch = 0, _Roll = 0;

        bool IsDirtyYawPitchRoll = true;

        public Rotation(float yaw, float pitch, float roll)
        {
            Quaternion.CreateFromYawPitchRoll(yaw, pitch, roll, out _Quaternion);

            _Yaw = yaw;
            _Pitch = pitch;
            _Roll = roll;

            IsDirtyYawPitchRoll = false;
        }

        public Rotation()
            : this(0, 0, 0)
        {
        }

        public void RotateAxis(Vector3 axis, float angle, bool relative)
        {
            if (angle == 0)
            {
                return;
            }
            if (relative)
            {
                Vector3.Transform(ref axis, ref _Quaternion, out axis);
            }
            Quaternion q = Quaternion.CreateFromAxisAngle(axis, angle);

            Quaternion.Multiply(ref q, ref _Quaternion, out q);
            Quaternion.Normalize(ref q, out _Quaternion);

            IsDirtyYawPitchRoll = true;
        }

        public void RotateAxis(ref Vector3 axis, float angle, bool relative)
        {
            if (angle == 0)
            {
                return;
            }
            
            Vector3 a = axis;
            if (relative)
            {
                Vector3.Transform(ref axis, ref _Quaternion, out a);
            }
            Quaternion q = Quaternion.CreateFromAxisAngle(a, angle);

            Quaternion.Multiply(ref q, ref _Quaternion, out q);
            Quaternion.Normalize(ref q, out _Quaternion);

            IsDirtyYawPitchRoll = true;
        }

        public void RotateY(float angle, bool relative)
        {
            if (angle == 0)
            {
                return;
            }
            Vector3 axis = Vector3.Up;
            if (relative)
            {
                Vector3.Transform(ref UNIT_Y, ref _Quaternion, out axis);
            }
            Quaternion q = Quaternion.CreateFromAxisAngle(axis, angle);

            Quaternion.Multiply(ref q, ref _Quaternion, out q);
            Quaternion.Normalize(ref q, out _Quaternion);
            _Yaw += angle;
        }

        public void RotateX(float angle, bool relative)
        {
            if (angle == 0)
            {
                return;
            }
            Vector3 axis = UNIT_X;
            if (relative)
            {
                Vector3.Transform(ref UNIT_X, ref _Quaternion, out axis);
            }
            Quaternion q = Quaternion.CreateFromAxisAngle(axis, angle);

            Quaternion.Multiply(ref q, ref _Quaternion, out q);
            Quaternion.Normalize(ref q, out _Quaternion);
            _Pitch += angle;
        }

        public void RotateZ(float angle, bool relative)
        {
            if (angle == 0)
            {
                return;
            }
            Vector3 axis = UNIT_Z;
            if (relative)
            {
                Vector3.Transform(ref UNIT_Z, ref _Quaternion, out axis);
            }
            Quaternion q = Quaternion.CreateFromAxisAngle(axis, angle);

            Quaternion.Multiply(ref q, ref _Quaternion, out q);
            Quaternion.Normalize(ref q, out _Quaternion);
            _Roll += angle;
        }

        public void SetLookDirection(Vector3 direction)
        {
            Matrix matrix = Matrix.CreateLookAt(Vector3.Zero, direction, UNIT_Y);
            Matrix.Invert(ref matrix, out matrix);

            Quaternion.CreateFromRotationMatrix(ref matrix, out _Quaternion);
            IsDirtyYawPitchRoll = true;
        }

        public void Transform(ref Vector3 position, out Vector3 result)
        {
            Vector3.Transform(ref position, ref _Quaternion, out result);
        }

        public float Yaw
        {
            get
            {
                CalculateYawPitchRoll();
                return _Yaw;
            }
            set
            {
                CalculateYawPitchRoll();
                _Yaw = value;
                Quaternion.CreateFromYawPitchRoll(_Yaw, _Pitch, _Roll, out _Quaternion);
            }
        }

        public float Pitch
        {
            get
            {
                CalculateYawPitchRoll();
                return _Pitch;
            }
            set
            {
                CalculateYawPitchRoll();
                _Pitch = value;
                Quaternion.CreateFromYawPitchRoll(_Yaw, _Pitch, _Roll, out _Quaternion);
            }
        }

        public float Roll
        {
            get
            {
                CalculateYawPitchRoll();
                return _Roll;
            }
            set
            {
                CalculateYawPitchRoll();
                _Roll = value;

                Quaternion.CreateFromYawPitchRoll(_Yaw, _Pitch, _Roll, out _Quaternion);
            }
        }

        public void GetMatrix(out Matrix rotation)
        {
            Matrix.CreateFromQuaternion(ref _Quaternion, out rotation);
        }

        public Matrix GetMatrix()
        {
            return Matrix.CreateFromQuaternion(_Quaternion);
        }

        public Quaternion Quaternion
        {
            get
            {
                return _Quaternion;
            }
            set
            {
                _Quaternion = value;
                IsDirtyYawPitchRoll = true;
            }
        }

        

        private void CalculateYawPitchRoll()
        {
            if (IsDirtyYawPitchRoll)
            {
                Vector3 view = Vector3.Zero;
                view.X = _Quaternion.X;
                view.Y = _Quaternion.Y;
                view.Z = _Quaternion.Z;
                float save = 0;

                save = view.Y;
                view.Y = 0;
                _Yaw = -2.0f * (float)Math.Acos(Vector3.Dot(Vector3.Forward, view));

                view.Y = save;

                save = view.X;
                view.X = 0;

                _Pitch = -2.0f * (float)Math.Acos(Vector3.Dot(Vector3.UnitY, view)) + MathHelper.Pi;

                save = view.X;
                view.X = save;

                save = view.Z;
                view.Z = 0;

                _Roll = -2.0f * (float)Math.Acos(Vector3.Dot(Vector3.UnitX, view));
                IsDirtyYawPitchRoll = false;
            }

            
        }
        public void CalculateYawPitchRoll(out float yaw, out float pitch, out float roll)
        {
            CalculateYawPitchRoll();

            yaw = _Yaw;
            pitch = _Pitch;
            roll = _Roll;
        }
    }
}
