using System;

namespace BulletX.LinerMath
{
    [Flags]
    public enum DebugDrawModes
    {
        DBG_NoDebug = 0,
        DBG_DrawWireframe = 1,
        DBG_DrawAabb = 2,
        DBG_DrawFeaturesText = 4,
        DBG_DrawContactPoints = 8,
        DBG_NoDeactivation = 16,
        DBG_NoHelpText = 32,
        DBG_DrawText = 64,
        DBG_ProfileTimings = 128,
        DBG_EnableSatComparison = 256,
        DBG_DisableBulletLCP = 512,
        DBG_EnableCCD = 1024,
        DBG_DrawConstraints = (1 << 11),
        DBG_DrawConstraintLimits = (1 << 12),
        DBG_FastWireframe = (1 << 13),
        DBG_MAX_DEBUG_DRAW_MODE
    };
    public abstract class IDebugDraw
    {
        public IDebugDraw() { }
        public abstract void drawLine(btVector3 from, btVector3 to, btVector3 color);
        public virtual void drawLine(btVector3 from, btVector3 to, btVector3 fromColor, btVector3 toColor)
        {
            drawLine(from, to, fromColor);
        }

        public void drawSphere(float radius, btTransform transform, btVector3 color)
        {
            btVector3 start = transform.Origin;

            btVector3 xoffs = transform.Basis * new btVector3(radius, 0, 0);
            btVector3 yoffs = transform.Basis * new btVector3(0, radius, 0);
            btVector3 zoffs = transform.Basis * new btVector3(0, 0, radius);

            // XY 
            drawLine(start - xoffs, start + yoffs, color);
            drawLine(start + yoffs, start + xoffs, color);
            drawLine(start + xoffs, start - yoffs, color);
            drawLine(start - yoffs, start - xoffs, color);

            // XZ
            drawLine(start - xoffs, start + zoffs, color);
            drawLine(start + zoffs, start + xoffs, color);
            drawLine(start + xoffs, start - zoffs, color);
            drawLine(start - zoffs, start - xoffs, color);

            // YZ
            drawLine(start - yoffs, start + zoffs, color);
            drawLine(start + zoffs, start + yoffs, color);
            drawLine(start + yoffs, start - zoffs, color);
            drawLine(start - zoffs, start - yoffs, color);
        }

        public virtual void drawSphere(btVector3 p, float radius, btVector3 color)
        {
            btTransform tr = btTransform.Identity;
            tr.Origin = p;
            drawSphere(radius, tr, color);
        }

        public virtual void drawTriangle(btVector3 v0, btVector3 v1, btVector3 v2, btVector3 n0, btVector3 n1, btVector3 n2, btVector3 color, float alpha)
        {
            drawTriangle(v0, v1, v2, color, alpha);
        }
        public virtual void drawTriangle(btVector3 v0, btVector3 v1, btVector3 v2, btVector3 color, float alpha)
        {
            drawLine(v0, v1, color);
            drawLine(v1, v2, color);
            drawLine(v2, v0, color);
        }

        public abstract void	drawContactPoint(btVector3 PointOnB,btVector3 normalOnB,float distance,int lifeTime,btVector3 color);

	    public abstract void reportErrorWarning(string warningString);
        
	    public abstract void	draw3dText(btVector3 location,string textString);

        public abstract DebugDrawModes DebugMode { get; set; }

        public virtual void drawAabb(btVector3 from, btVector3 to, btVector3 color)
        {

            btVector3 halfExtents = (to - from) * 0.5f;
            btVector3 center = (to + from) * 0.5f;
            int i, j;

            btVector3 edgecoord = new btVector3(1f, 1f, 1f), pa, pb;
            for (i = 0; i < 4; i++)
            {
                for (j = 0; j < 3; j++)
                {
                    pa = new btVector3(edgecoord[0] * halfExtents[0], edgecoord[1] * halfExtents[1],
                        edgecoord[2] * halfExtents[2]);
                    pa += center;

                    int othercoord = j % 3;
                    edgecoord[othercoord] *= -1f;
                    pb = new btVector3(edgecoord[0] * halfExtents[0], edgecoord[1] * halfExtents[1],
                        edgecoord[2] * halfExtents[2]);
                    pb += center;

                    drawLine(pa, pb, color);
                }
                edgecoord = new btVector3(-1f, -1f, -1f);
                if (i < 3)
                    edgecoord[i] *= -1f;
            }
        }
	
        public virtual void drawTransform(btTransform transform, float orthoLen)
        {
            btVector3 start = transform.Origin;
            drawLine(start, start + transform.Basis * new btVector3(orthoLen, 0, 0), new btVector3(0.7f, 0, 0));
            drawLine(start, start + transform.Basis * new btVector3(0, orthoLen, 0), new btVector3(0, 0.7f, 0));
            drawLine(start, start + transform.Basis * new btVector3(0, 0, orthoLen), new btVector3(0, 0, 0.7f));
        }
        public virtual void drawArc(btVector3 center, btVector3 normal, btVector3 axis, float radiusA, float radiusB, float minAngle, float maxAngle, 
				btVector3 color, bool drawSect)
	    {
            drawArc(center,normal,axis,radiusA,radiusB,minAngle,maxAngle,color,drawSect,10f);
        }
        public virtual void drawArc(btVector3 center, btVector3 normal, btVector3 axis, float radiusA, float radiusB, float minAngle, float maxAngle, 
				btVector3 color, bool drawSect, float stepDegrees)
	    {
		    btVector3 vx = axis;
		    btVector3 vy = normal.cross(axis);
		    float step = stepDegrees * BulletGlobal.SIMD_RADS_PER_DEG;
		    int nSteps = (int)((maxAngle - minAngle) / step);
		    if(nSteps==0) nSteps = 1;
		    btVector3 prev = center + radiusA * vx * (float)Math.Cos(minAngle) + radiusB * vy * (float)Math.Sin(minAngle);
		    if(drawSect)
		    {
			    drawLine(center, prev, color);
		    }
		    for(int i = 1; i <= nSteps; i++)
		    {
			    float angle = minAngle + (maxAngle - minAngle) * (float)(i) / (float)(nSteps);
			    btVector3 next = center + radiusA * vx * (float)Math.Cos(angle) + radiusB * vy * (float)Math.Cos(angle);
			    drawLine(prev, next, color);
			    prev = next;
		    }
		    if(drawSect)
		    {
			    drawLine(center, prev, color);
		    }
	    }
	    public virtual void drawSpherePatch(btVector3 center, btVector3 up,btVector3 axis, float radius, 
		    float minTh, float maxTh, float minPs, float maxPs, btVector3 color)
	    {
            drawSpherePatch(center,up,axis,radius,minTh,maxTh,minPs,maxPs,color, 10f);
        }
        public virtual unsafe void drawSpherePatch(btVector3 center, btVector3 up,btVector3 axis, float radius, 
		    float minTh, float maxTh, float minPs, float maxPs, btVector3 color, float stepDegrees)
	    {
		    //btVector3* vA=stackalloc btVector3[74];
		    //btVector3* vB=stackalloc btVector3[74];
            StackPtr<btVector3> vA = StackPtr<btVector3>.Allocate(74);
            StackPtr<btVector3> vB = StackPtr<btVector3>.Allocate(74);
            try
            {
                fixed (btVector3* vA_ptr = &vA.Array[0], vB_ptr = &vB.Array[0])
                {
                    btVector3* pvA = vA_ptr, pvB = vB_ptr, pT;
                    btVector3 npole = center + up * radius;
                    btVector3 spole = center - up * radius;
                    btVector3 arcStart = btVector3.Zero;
                    float step = stepDegrees * BulletGlobal.SIMD_RADS_PER_DEG;
                    btVector3 kv = up;
                    btVector3 iv = axis;
                    btVector3 jv = kv.cross(iv);
                    bool drawN = false;
                    bool drawS = false;
                    if (minTh <= -BulletGlobal.SIMD_HALF_PI)
                    {
                        minTh = -BulletGlobal.SIMD_HALF_PI + step;
                        drawN = true;
                    }
                    if (maxTh >= BulletGlobal.SIMD_HALF_PI)
                    {
                        maxTh = BulletGlobal.SIMD_HALF_PI - step;
                        drawS = true;
                    }
                    if (minTh > maxTh)
                    {
                        minTh = -BulletGlobal.SIMD_HALF_PI + step;
                        maxTh = BulletGlobal.SIMD_HALF_PI - step;
                        drawN = drawS = true;
                    }
                    int n_hor = (int)((maxTh - minTh) / step) + 1;
                    if (n_hor < 2) n_hor = 2;
                    float step_h = (maxTh - minTh) / (float)(n_hor - 1);
                    bool isClosed = false;
                    if (minPs > maxPs)
                    {
                        minPs = -BulletGlobal.SIMD_PI + step;
                        maxPs = BulletGlobal.SIMD_PI;
                        isClosed = true;
                    }
                    else if ((maxPs - minPs) >= BulletGlobal.SIMD_PI * (2f))
                    {
                        isClosed = true;
                    }
                    else
                    {
                        isClosed = false;
                    }
                    int n_vert = (int)((maxPs - minPs) / step) + 1;
                    if (n_vert < 2) n_vert = 2;
                    float step_v = (maxPs - minPs) / (float)(n_vert - 1);
                    for (int i = 0; i < n_hor; i++)
                    {
                        float th = minTh + (float)(i) * step_h;
                        float sth = radius * (float)Math.Sin(th);
                        float cth = radius * (float)Math.Cos(th);
                        for (int j = 0; j < n_vert; j++)
                        {
                            float psi = minPs + (float)(j) * step_v;
                            float sps = (float)Math.Sin(psi);
                            float cps = (float)Math.Cos(psi);
                            pvB[j] = center + cth * cps * iv + cth * sps * jv + sth * kv;
                            if (i != 0)
                            {
                                drawLine(pvA[j], pvB[j], color);
                            }
                            else if (drawS)
                            {
                                drawLine(spole, pvB[j], color);
                            }
                            if (j != 0)
                            {
                                drawLine(pvB[j - 1], pvB[j], color);
                            }
                            else
                            {
                                arcStart = pvB[j];
                            }
                            if ((i == (n_hor - 1)) && drawN)
                            {
                                drawLine(npole, pvB[j], color);
                            }
                            if (isClosed)
                            {
                                if (j == (n_vert - 1))
                                {
                                    drawLine(arcStart, pvB[j], color);
                                }
                            }
                            else
                            {
                                if (((i == 0) || (i == (n_hor - 1))) && ((j == 0) || (j == (n_vert - 1))))
                                {
                                    drawLine(center, pvB[j], color);
                                }
                            }
                        }
                        pT = pvA; pvA = pvB; pvB = pT;
                    }
                }
            }
            finally
            {
                vA.Dispose();
                vB.Dispose();
            }
	    }
    	
	    public virtual void drawBox(btVector3 bbMin, btVector3 bbMax, btVector3 color)
	    {
		    drawLine(new btVector3(bbMin[0], bbMin[1], bbMin[2]),new  btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
		    drawLine(new btVector3(bbMax[0], bbMin[1], bbMin[2]),new  btVector3(bbMax[0], bbMax[1], bbMin[2]), color);
		    drawLine(new btVector3(bbMax[0], bbMax[1], bbMin[2]),new  btVector3(bbMin[0], bbMax[1], bbMin[2]), color);
		    drawLine(new btVector3(bbMin[0], bbMax[1], bbMin[2]),new  btVector3(bbMin[0], bbMin[1], bbMin[2]), color);
		    drawLine(new btVector3(bbMin[0], bbMin[1], bbMin[2]),new  btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
		    drawLine(new btVector3(bbMax[0], bbMin[1], bbMin[2]),new  btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
		    drawLine(new btVector3(bbMax[0], bbMax[1], bbMin[2]),new  btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
		    drawLine(new btVector3(bbMin[0], bbMax[1], bbMin[2]),new  btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
		    drawLine(new btVector3(bbMin[0], bbMin[1], bbMax[2]),new  btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
		    drawLine(new btVector3(bbMax[0], bbMin[1], bbMax[2]),new  btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
		    drawLine(new btVector3(bbMax[0], bbMax[1], bbMax[2]),new  btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
		    drawLine(new btVector3(bbMin[0], bbMax[1], bbMax[2]),new  btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
	    }
	    public virtual void drawBox(btVector3 bbMin, btVector3 bbMax,btTransform trans, btVector3 color)
	    {
		    drawLine(trans * new btVector3(bbMin[0], bbMin[1], bbMin[2]), trans *new  btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
		    drawLine(trans * new btVector3(bbMax[0], bbMin[1], bbMin[2]), trans *new  btVector3(bbMax[0], bbMax[1], bbMin[2]), color);
		    drawLine(trans * new btVector3(bbMax[0], bbMax[1], bbMin[2]), trans *new  btVector3(bbMin[0], bbMax[1], bbMin[2]), color);
		    drawLine(trans * new btVector3(bbMin[0], bbMax[1], bbMin[2]), trans *new  btVector3(bbMin[0], bbMin[1], bbMin[2]), color);
		    drawLine(trans * new btVector3(bbMin[0], bbMin[1], bbMin[2]), trans *new  btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
		    drawLine(trans * new btVector3(bbMax[0], bbMin[1], bbMin[2]), trans *new  btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
		    drawLine(trans * new btVector3(bbMax[0], bbMax[1], bbMin[2]), trans *new  btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
		    drawLine(trans * new btVector3(bbMin[0], bbMax[1], bbMin[2]), trans *new  btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
		    drawLine(trans * new btVector3(bbMin[0], bbMin[1], bbMax[2]), trans *new  btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
		    drawLine(trans * new btVector3(bbMax[0], bbMin[1], bbMax[2]), trans *new  btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
		    drawLine(trans * new btVector3(bbMax[0], bbMax[1], bbMax[2]), trans *new  btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
		    drawLine(trans * new btVector3(bbMin[0], bbMax[1], bbMax[2]), trans *new  btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
	    }
    }
}
