using System;
using System.Diagnostics;

namespace BulletX.LinerMath
{
    public struct btVector3
    {
        #region メンバ変数
        public float X;
        public float Y;
        public float Z;
        public float W;
        public float this[int index]
        {
            get
            {
                switch (index)
                {
                    case 0:
                        return X;
                    case 1:
                        return Y;
                    case 2:
                        return Z;
                    case 3:
                        return W;
                }
                throw new ArgumentOutOfRangeException();
            }
            set
            {
                switch (index)
                {
                    case 0:
                        X = value;
                        return;
                    case 1:
                        Y = value;
                        return;
                    case 2:
                        Z = value;
                        return;
                    case 3:
                        W = value;
                        return;
                }
                throw new ArgumentOutOfRangeException();
            }
        }
        #endregion
        public btVector3(float x, float y, float z)
        {
            X = x;
            Y = y;
            Z = z;
            W = 0;
        }
        public btVector3(float[] param)
        {
            X = param[0];
            Y = param[1];
            Z = param[2];
            if (param.Length >= 4)
                W = param[3];
            else
                W = 0;
        }

        //statics
        public static btVector3 Zero { get { return new btVector3(0, 0, 0); } }

        #region 演算子オーバーロード
        public static btVector3 operator +(btVector3 v1, btVector3 v2)
        {
            return new btVector3(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z);
        }
        public static btVector3 operator -(btVector3 value1, btVector3 value2)
        {
            return new btVector3(value1.X - value2.X, value1.Y - value2.Y, value1.Z - value2.Z);
        }
        public static btVector3 operator /(btVector3 value1, btVector3 value2)
        {
            Debug.Assert(value2.X != 0 && value2.Y != 0 && value2.Z != 0);
            return new btVector3(value1.X / value2.X, value1.Y / value2.Y, value1.Z / value2.Z);
        }
        public static btVector3 operator *(btVector3 value1, float value2)
        {
            return new btVector3(value1.X * value2, value1.Y * value2, value1.Z * value2);
        }
        public static btVector3 operator *(float value1, btVector3 value2)
        {
            return value2 * value1;
        }
        public static btVector3 operator /(btVector3 value1, float value2)
        {
            Debug.Assert(value2 != 0);
            return new btVector3(value1.X / value2, value1.Y / value2, value1.Z / value2);
        }
        public static btVector3 operator -(btVector3 value)
        {
            return new btVector3(-value.X, -value.Y, -value.Z);
        }
        public static btVector3 operator *(btVector3 v1, btVector3 v2)
        {
            return new btVector3(v1.X * v2.X, v1.Y * v2.Y, v1.Z * v2.Z);
        }
        public static bool Equals(ref btVector3 v1, ref btVector3 v2)
        {
            return (v1.X == v2.X && v1.Y == v2.Y && v1.Z == v2.Z);
        }
        #endregion

        //プロパティ
        public float Length
        {
            get { return (float)Math.Sqrt(Length2); }
        }
        public float Length2
        {
            get { return dot(this); }
        }


        //演算系
        public void setValue(float x, float y, float z)
        {
            X = x;
            Y = y;
            Z = z;
            W = 0;
        }
        public float dot(btVector3 v)
        {
            return X * v.X + Y * v.Y + Z * v.Z;
        }
        public btVector3 normalized()
        {
            return this / Length;
        }
        public void setZero()
        {
            setValue(0f, 0f, 0f);
        }
        public btVector3 cross(btVector3 v)
        {
            return new btVector3(
                Y * v.Z - Z * v.Y,
                Z * v.X - X * v.Z,
                X * v.Y - Y * v.X);
        }
        public btVector3 normalize()
        {
            return (this /= Length);
        }

        public static btVector3 Transform(btVector3 vector, btTransform trans)
        {
            return new btVector3(trans.Basis[0].dot(vector) + trans.Origin.X,
                trans.Basis[1].dot(vector) + trans.Origin.Y,
                trans.Basis[2].dot(vector) + trans.Origin.Z);
        }

        public static void PlaneSpace1(ref btVector3 n, out btVector3 p, out btVector3 q)
        {
            if ((float)Math.Abs(n.Z) > BulletGlobal.SIMDSQRT12)
            {
                // choose p in y-z plane
                float a = n[1] * n[1] + n[2] * n[2];
                float k = 1f / (float)Math.Sqrt(a);
                p = new btVector3(0, -n[2] * k, n[1] * k);
                // set q = n x p
                q = new btVector3(a * k, -n[0] * p[2], n[0] * p[1]);
            }
            else
            {
                // choose p in x-y plane
                float a = n.X * n.X + n.Y * n.Y;
                float k = 1f / (float)Math.Sqrt(a);
                p = new btVector3(-n.Y * k, n.X * k, 0);
                // set q = n x p
                q = new btVector3(-n.Z * p.Y, n.Z * p.X, a * k);
            }
        }

        public float distance2(btVector3 v)
        {
            return (v - this).Length2;
        }

        public static float dot(btVector3 v1, btVector3 v2)
        {
            return v1.dot(v2);
        }
        public static btVector3 cross(btVector3 v1, btVector3 v2)
        {
            return v1.cross(v2);
        }

        public btVector3 absolute()
        {
            return new btVector3(
            (float)Math.Abs(X),
            (float)Math.Abs(Y),
            (float)Math.Abs(Z));
        }
        public Microsoft.Xna.Framework.Vector3 CreateXNAVector()
        {
            return new Microsoft.Xna.Framework.Vector3(X, Y, Z);
        }
    }
}
