using System.Diagnostics;
using BulletX.LinerMath;

namespace BulletX.BulletDynamics.ConstraintSolver
{
    public struct JacobianEntry
    {
        public btVector3 m_linearJointAxis;
        public btVector3 m_aJ;
        public btVector3 m_bJ;
        public btVector3 m_0MinvJt;
        public btVector3 m_1MinvJt;
        //Optimization: can be stored in the w/last component of one of the vectors
        public float m_Adiag;

        public JacobianEntry(
        btMatrix3x3 world2A,
        btMatrix3x3 world2B,
        btVector3 rel_pos1, btVector3 rel_pos2,
        btVector3 jointAxis,
        btVector3 inertiaInvA,
        float massInvA,
        btVector3 inertiaInvB,
        float massInvB)
        {
            m_linearJointAxis = jointAxis;
            m_aJ = world2A * (rel_pos1.cross(m_linearJointAxis));
            m_bJ = world2B * (rel_pos2.cross(-m_linearJointAxis));
            m_0MinvJt = inertiaInvA * m_aJ;
            m_1MinvJt = inertiaInvB * m_bJ;
            m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);

            Debug.Assert(m_Adiag > 0.0f);
        }

        //angular constraint between two different rigidbodies
        public JacobianEntry(btVector3 jointAxis,
            btMatrix3x3 world2A,
            btMatrix3x3 world2B,
            btVector3 inertiaInvA,
            btVector3 inertiaInvB)
        {
            m_linearJointAxis = btVector3.Zero;//(btScalar(0.),btScalar(0.),btScalar(0.));
            m_aJ = world2A * jointAxis;
            m_bJ = world2B * -jointAxis;
            m_0MinvJt = inertiaInvA * m_aJ;
            m_1MinvJt = inertiaInvB * m_bJ;
            m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);

            Debug.Assert(m_Adiag > 0.0f);
        }

        //angular constraint between two different rigidbodies
        public JacobianEntry(btVector3 axisInA,
            btVector3 axisInB,
            btVector3 inertiaInvA,
            btVector3 inertiaInvB)
        {
            m_linearJointAxis = btVector3.Zero;//(btScalar(0.),btScalar(0.),btScalar(0.));
            m_aJ = axisInA;
            m_bJ = -axisInB;
            m_0MinvJt = inertiaInvA * m_aJ;
            m_1MinvJt = inertiaInvB * m_bJ;
            m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);

            Debug.Assert(m_Adiag > 0.0f);
        }

        //constraint on one rigidbody
        public JacobianEntry(
            btMatrix3x3 world2A,
            btVector3 rel_pos1, btVector3 rel_pos2,
            btVector3 jointAxis,
            btVector3 inertiaInvA,
            float massInvA)
        {
            m_linearJointAxis = jointAxis;
            m_aJ = world2A * (rel_pos1.cross(jointAxis));
            m_bJ = world2A * (rel_pos2.cross(-jointAxis));
            m_0MinvJt = inertiaInvA * m_aJ;
            m_1MinvJt = btVector3.Zero;//(btScalar(0.),btScalar(0.),btScalar(0.));
            m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);

            Debug.Assert(m_Adiag > 0.0f);
        }

        float Diagonal { get { return m_Adiag; } }

        // for two constraints on the same rigidbody (for example vehicle friction)
        float getNonDiagonal(JacobianEntry jacB, float massInvA)
        {
            float lin = massInvA * this.m_linearJointAxis.dot(jacB.m_linearJointAxis);
            float ang = this.m_0MinvJt.dot(jacB.m_aJ);
            return lin + ang;
        }



        // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
        float getNonDiagonal(JacobianEntry jacB, float massInvA, float massInvB)
        {
            btVector3 lin = this.m_linearJointAxis * jacB.m_linearJointAxis;
            btVector3 ang0 = this.m_0MinvJt * jacB.m_aJ;
            btVector3 ang1 = this.m_1MinvJt * jacB.m_bJ;
            btVector3 lin0 = massInvA * lin;
            btVector3 lin1 = massInvB * lin;
            btVector3 sum = ang0 + ang1 + lin0 + lin1;
            return sum[0] + sum[1] + sum[2];
        }

        float getRelativeVelocity(btVector3 linvelA, btVector3 angvelA, btVector3 linvelB, btVector3 angvelB)
        {
            btVector3 linrel = linvelA - linvelB;
            btVector3 angvela = angvelA * m_aJ;
            btVector3 angvelb = angvelB * m_bJ;
            linrel *= m_linearJointAxis;
            angvela += angvelb;
            angvela += linrel;
            float rel_vel2 = angvela[0] + angvela[1] + angvela[2];
            return rel_vel2 + BulletGlobal.SIMD_EPSILON;
        }
        //private:


    }
}
