using BulletX.BulletCollision.CollisionShapes;
using BulletX.LinerMath;
using tShape = BulletX.BulletCollision.NarrowPhaseCollision.MinkowskiDiff;
using U = System.UInt32;
	
namespace BulletX.BulletCollision.NarrowPhaseCollision
{
    static class GjkEpaSolver2
    {
        const float GJK_MIN_DISTANCE = 0.0001f;
        public struct sResults
        {
            public enum eStatus
            {
                Separated,		/* Shapes doesnt penetrate												*/
                Penetrating,	/* Shapes are penetrating												*/
                GJK_Failed,		/* GJK phase fail, no big issue, shapes are probably just 'touching'	*/
                EPA_Failed		/* EPA phase fail, bigger problem, need to save parameters, and debug	*/
            }
            public eStatus status;
            public btVector3 witnesses0;
            public btVector3 witnesses1;
            public btVector3 normal;
            public float distance;
        }
        public static bool Distance(ConvexShape shape0, btTransform wtrs0, ConvexShape shape1, btTransform wtrs1, btVector3 guess, ref sResults results)
        {
            tShape shape = new MinkowskiDiff();
            Initialize(shape0, wtrs0, shape1, wtrs1, ref results, ref shape, false);
            using(GJK gjk = GJK.CreateFromPool())
            {
                GJK.eStatus gjk_status = gjk.Evaluate(shape, guess);
                if (gjk_status == GJK.eStatus.Valid)
                {
                    btVector3 w0 = new btVector3(0, 0, 0);
                    btVector3 w1 = new btVector3(0, 0, 0);
                    for (U i = 0; i < gjk.m_simplex.rank; ++i)
                    {
                        float p = gjk.m_simplex.p[i];
                        w0 += shape.Support(gjk.m_simplex.c[i].d, 0) * p;
                        w1 += shape.Support(-gjk.m_simplex.c[i].d, 1) * p;
                    }
                    results.witnesses0 = wtrs0 * w0;
                    results.witnesses1 = wtrs0 * w1;
                    results.normal = w0 - w1;
                    results.distance = results.normal.Length;
                    results.normal /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1;
                    return (true);
                }
                else
                {
                    results.status = gjk_status == GJK.eStatus.Inside ?
                        sResults.eStatus.Penetrating :
                    sResults.eStatus.GJK_Failed;
                    return (false);
                }
            }
        }

        public static bool Penetration(ConvexShape shape0, btTransform wtrs0, ConvexShape shape1, btTransform wtrs1, btVector3 guess, ref sResults results)
        {
            return Penetration(shape0, wtrs0, shape1, wtrs1, guess, ref results, true);
        }
        public static bool Penetration(ConvexShape shape0, btTransform wtrs0, ConvexShape shape1, btTransform wtrs1, btVector3 guess, ref sResults results, bool usemargins)
        {
            tShape			shape=new MinkowskiDiff();
	        Initialize(shape0,wtrs0,shape1,wtrs1,ref results,ref shape,usemargins);
            using (GJK gjk = GJK.CreateFromPool())
            {
                GJK.eStatus gjk_status = gjk.Evaluate(shape, -guess);
                switch (gjk_status)
                {
                    case GJK.eStatus.Inside:
                        {
                            using(EPA epa = EPA.CreateFromPool())
                            {
                                EPA.eStatus epa_status = epa.Evaluate(gjk, -guess);
                                if (epa_status != EPA.eStatus.Failed)
                                {
                                    btVector3 w0 = new btVector3(0, 0, 0);
                                    for (U i = 0; i < epa.m_result.rank; ++i)
                                    {
                                        w0 += shape.Support(epa.m_result.c[i].d, 0) * epa.m_result.p[i];
                                    }
                                    results.status = sResults.eStatus.Penetrating;
                                    results.witnesses0 = wtrs0 * w0;
                                    results.witnesses1 = wtrs0 * (w0 - epa.m_normal * epa.m_depth);
                                    results.normal = -epa.m_normal;
                                    results.distance = -epa.m_depth;
                                    return (true);
                                }
                                else results.status = sResults.eStatus.EPA_Failed;
                            }
                        }
                        break;
                    case GJK.eStatus.Failed:
                        results.status = sResults.eStatus.GJK_Failed;
                        break;
                    default:
                        {
                            break;
                        }
                }
                return (false);
            }
            
        }
        static void	Initialize(ConvexShape shape0,btTransform wtrs0,
		    ConvexShape shape1,btTransform wtrs1,
		    ref GjkEpaSolver2.sResults results,
		    ref tShape shape,
		    bool withmargins)
	    {
		    /* Results		*/ 
		    results.witnesses0	=
			    results.witnesses1	=	new btVector3(0,0,0);
		    results.status			=	GjkEpaSolver2.sResults.eStatus.Separated;
		    /* Shape		*/ 
		    shape.m_shapes0		=	shape0;
		    shape.m_shapes1		=	shape1;
		    shape.m_toshape1		=	wtrs1.Basis.transposeTimes(wtrs0.Basis);
		    shape.m_toshape0		=	wtrs0.inverseTimes(wtrs1);
		    shape.EnableMargin(withmargins);
	    }
    }
}
