using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;

namespace BulletX.LinerMath
{
    public struct btMatrix3x3
    {
        btVector3 m_el0, m_el1, m_el2;
        
        public static btMatrix3x3 Identity
        {
            get
            {
                return new btMatrix3x3(1, 0, 0,
                                        0, 1, 0,
                                        0, 0, 1);
            }
        }
        
        public btMatrix3x3(float xx, float xy, float xz,
		    float yx, float yy, float yz,
		    float zx, float zy, float zz)
	    {
            m_el0=new btVector3(xx, xy, xz);
            m_el1 = new btVector3(yx, yy, yz);
            m_el2 = new btVector3(zx, zy, zz);
        }
        public btMatrix3x3(btQuaternion q)
        {
            float d = q.Length2;
            Debug.Assert(d != 0.0f);
            float s = 2.0f / d;
            float xs = q.X * s, ys = q.Y * s, zs = q.Z * s;
            float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs;
            float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs;
            float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs;
            m_el0 = new btVector3(1.0f - (yy + zz), xy - wz, xz + wy);
            m_el1 = new btVector3(xy + wz, 1.0f - (xx + zz), yz - wx);
            m_el2 = new btVector3(xz - wy, yz + wx, 1.0f - (xx + yy));
        }
        public btMatrix3x3(ref Microsoft.Xna.Framework.Matrix m)
        {
            /*
            m_el[0].setValue(m[0], m[4], m[8]);
            m_el[1].setValue(m[1], m[5], m[9]);
            m_el[2].setValue(m[2], m[6], m[10]);*/
            /*
            matGL[0]=mat->M11;
            matGL[1]=mat->M12;
            matGL[2]=mat->M13;
            matGL[3]=mat->M14;//
            matGL[4]=mat->M21;
            matGL[5]=mat->M22;
            matGL[6]=mat->M23;
            matGL[7]=mat->M24;//
            matGL[8]=mat->M31;
            matGL[9]=mat->M32;
            matGL[10]=mat->M33;
            matGL[11]=mat->M34;//
            matGL[12]=mat->M41;
            matGL[13]=mat->M42;
            matGL[14]=mat->M43;
            matGL[15]=mat->M44;//
             */
            m_el0 = new btVector3(m.M11, m.M21, m.M31);
            m_el1 = new btVector3(m.M12, m.M22, m.M32);
            m_el2 = new btVector3(m.M13, m.M23, m.M33);
        }
        #region 演算子オーバーロード
        public btVector3 this[int i]
        {
            get
            {
                switch (i)
                {
                    case 0:
                        return m_el0;
                    case 1:
                        return m_el1;
                    case 2:
                        return m_el2;
                    default:
                        throw new ArgumentOutOfRangeException();
                }
            }
        }
        public static btVector3 operator * (btMatrix3x3 m, btVector3 v) 
        {
	        return new btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
        }
        public static btVector3 operator *(btVector3 v, btMatrix3x3 m)
        {
            return new btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
        }
        public static btMatrix3x3 operator *(btMatrix3x3 m1, btMatrix3x3 m2)
        {
            return new btMatrix3x3(
                m2.tdotx(m1[0]), m2.tdoty(m1[0]), m2.tdotz(m1[0]),
                m2.tdotx(m1[1]), m2.tdoty(m1[1]), m2.tdotz(m1[1]),
                m2.tdotx(m1[2]), m2.tdoty(m1[2]), m2.tdotz(m1[2]));
        }
        #endregion

        /** @brief Set the values of the matrix explicitly (row major)
	    *  @param xx Top left
	    *  @param xy Top Middle
	    *  @param xz Top Right
	    *  @param yx Middle Left
	    *  @param yy Middle Middle
	    *  @param yz Middle Right
	    *  @param zx Bottom Left
	    *  @param zy Bottom Middle
	    *  @param zz Bottom Right*/
	    public void setValue(float xx,float xy,float xz, 
		    float yx, float yy, float yz, 
		    float zx, float zy, float zz)
	    {
		    m_el0.setValue(xx,xy,xz);
		    m_el1.setValue(yx,yy,yz);
		    m_el2.setValue(zx,zy,zz);
	    }

        public void setIdentity()
        {
            setValue(1.0f, 0.0f, 0.0f,
                    0.0f, 1.0f, 0.0f,
                    0.0f, 0.0f, 1.0f);
        }

        public void getXNASubMatrix(out Microsoft.Xna.Framework.Matrix m)
        {
            /*m[0] = btScalar(m_el[0].x());
            m[1] = btScalar(m_el[1].x());
            m[2] = btScalar(m_el[2].x());
            m[3] = btScalar(0.0);
            m[4] = btScalar(m_el[0].y());
            m[5] = btScalar(m_el[1].y());
            m[6] = btScalar(m_el[2].y());
            m[7] = btScalar(0.0);
            m[8] = btScalar(m_el[0].z());
            m[9] = btScalar(m_el[1].z());
            m[10] = btScalar(m_el[2].z());
            m[11] = btScalar(0.0); */
            /*
            matGL[0]=mat->M11;
            matGL[1]=mat->M12;
            matGL[2]=mat->M13;
            matGL[3]=mat->M14;//
            matGL[4]=mat->M21;
            matGL[5]=mat->M22;
            matGL[6]=mat->M23;
            matGL[7]=mat->M24;//
            matGL[8]=mat->M31;
            matGL[9]=mat->M32;
            matGL[10]=mat->M33;
            matGL[11]=mat->M34;//
            matGL[12]=mat->M41;
            matGL[13]=mat->M42;
            matGL[14]=mat->M43;
            matGL[15]=mat->M44;//
             */
            m = Microsoft.Xna.Framework.Matrix.Identity;
            m.M11 = m_el0.X;
            m.M12 = m_el1.X;
            m.M13 = m_el2.X;
            m.M14 = 0;
            m.M21 = m_el0.Y;
            m.M22 = m_el1.Y;
            m.M23 = m_el2.Y;
            m.M24 = 0;
            m.M31 = m_el0.Z;
            m.M32 = m_el1.Z;
            m.M33 = m_el2.Z;
            m.M34 = 0;
        }
        /** @brief Get a column of the matrix as a vector 
	    *  @param i Column number 0 indexed */
        public btVector3 getColumn(int i)
        {
            return new btVector3(m_el0[i], m_el1[i], m_el2[i]);
        }
        /**@brief Get the matrix represented as a quaternion 
	    * @param q The quaternion which will be set */
        public void getRotation(out btQuaternion q)
        {
            StackPtr<float> temp = StackPtr<float>.Allocate(4);
            try
            {
                float trace = m_el0.X + m_el1.Y + m_el2.Z;
                //float[] temp = new float[4];

                if (trace > 0.0f)
                {
                    float s = (float)Math.Sqrt(trace + 1.0);
                    temp[3] = (s * 0.5f);
                    s = 0.5f / s;

                    temp[0] = ((m_el2.Y - m_el1.Z) * s);
                    temp[1] = ((m_el0.Z - m_el2.X) * s);
                    temp[2] = ((m_el1.X - m_el0.Y) * s);
                }
                else
                {
                    int i = m_el0.X < m_el1.Y ?
                        (m_el1.Y < m_el2.Z ? 2 : 1) :
                        (m_el0.X < m_el2.Z ? 2 : 0);
                    int j = (i + 1) % 3;
                    int k = (i + 2) % 3;

                    float s = (float)Math.Sqrt(this[i][i] - this[j][j] - this[k][k] + 1.0f);
                    temp[i] = s * 0.5f;
                    s = 0.5f / s;

                    temp[3] = (this[k][j] - this[j][k]) * s;
                    temp[j] = (this[j][i] + this[i][j]) * s;
                    temp[k] = (this[k][i] + this[i][k]) * s;
                }
                q = new btQuaternion(temp[0], temp[1], temp[2], temp[3]);
            }
            finally
            {
                temp.Dispose();
            }
        }
        /** @brief Set the matrix from a quaternion
	    *  @param q The Quaternion to match */
        public void setRotation(ref btQuaternion q)
        {
            float d = q.Length2;
            Debug.Assert(d != 0.0f);
            float s = 2.0f / d;
            float xs = q.X * s, ys = q.Y * s, zs = q.Z * s;
            float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs;
            float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs;
            float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs;
            setValue(1.0f - (yy + zz), xy - wz, xz + wy,
                xy + wz, 1.0f - (xx + zz), yz - wx,
                xz - wy, yz + wx, 1.0f - (xx + yy));
        }

        #region 演算系
        public btMatrix3x3 transpose() 
        {
	        return new btMatrix3x3(m_el0.X, m_el1.X, m_el2.X,
		        m_el0.Y, m_el1.Y, m_el2.Y,
		        m_el0.Z, m_el1.Z, m_el2.Z);
        }
        public float tdotx(btVector3 v) 
	    {
		    return m_el0.X * v.X + m_el1.X * v.Y + m_el2.X * v.Z;
	    }
        public float tdoty(btVector3 v) 
	    {
		    return m_el0.Y * v.X + m_el1.Y * v.Y + m_el2.Y * v.Z;
	    }
	    public float tdotz(btVector3 v) 
	    {
		    return m_el0.Z * v.X + m_el1.Z * v.Y + m_el2.Z * v.Z;
	    }
        public btMatrix3x3 scaled(btVector3 s)
	    {
		    return new btMatrix3x3(m_el0.X * s.X, m_el0.Y * s.Y, m_el0.Z * s.Z,
			    m_el1.X * s.X, m_el1.Y * s.Y, m_el1.Z * s.Z,
			    m_el2.X * s.X, m_el2.Y * s.Y, m_el2.Z * s.Z);
	    }
        public btMatrix3x3 absolute()
        {
            return new btMatrix3x3(
                Math.Abs(m_el0.X), Math.Abs(m_el0.Y), Math.Abs(m_el0.Z),
                Math.Abs(m_el1.X), Math.Abs(m_el1.Y), Math.Abs(m_el1.Z),
                Math.Abs(m_el2.X), Math.Abs(m_el2.Y), Math.Abs(m_el2.Z));
        }
        public btMatrix3x3 inverse()
        {
	        btVector3 co=new btVector3(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
	        float det = this[0].dot(co);
	        Debug.Assert(det != 0.0);
	        float s = 1.0f / det;
	        return new btMatrix3x3(co.X * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
		        co.Y * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
		        co.Z * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
        }
        /**@brief Calculate the matrix cofactor 
	    * @param r1 The first row to use for calculating the cofactor
	    * @param c1 The first column to use for calculating the cofactor
	    * @param r1 The second row to use for calculating the cofactor
	    * @param c1 The second column to use for calculating the cofactor
	    * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
	    */
	    float cofac(int r1, int c1, int r2, int c2) 
	    {
		    return this[r1][c1] * this[r2][c2] - this[r1][c2] * this[r2][c1];
	    }
        #endregion

        public btMatrix3x3 transposeTimes(btMatrix3x3 m)
        {
	        return new btMatrix3x3(
		        m_el0.X * m[0].X + m_el1.X * m[1].X + m_el2.X * m[2].X,
		        m_el0.X * m[0].Y + m_el1.X * m[1].Y + m_el2.X * m[2].Y,
		        m_el0.X * m[0].Z + m_el1.X * m[1].Z + m_el2.X * m[2].Z,
		        m_el0.Y * m[0].X + m_el1.Y * m[1].X + m_el2.Y * m[2].X,
		        m_el0.Y * m[0].Y + m_el1.Y * m[1].Y + m_el2.Y * m[2].Y,
		        m_el0.Y * m[0].Z + m_el1.Y * m[1].Z + m_el2.Y * m[2].Z,
		        m_el0.Z * m[0].X + m_el1.Z * m[1].X + m_el2.Z * m[2].X,
		        m_el0.Z * m[0].Y + m_el1.Z * m[1].Y + m_el2.Z * m[2].Y,
		        m_el0.Z * m[0].Z + m_el1.Z * m[1].Z + m_el2.Z * m[2].Z);
        }
    }
}
