#include <game.h>
#define GEKKO
#include "rvl/mtx.h"
#include "rvl/GXEnum.h"
#include "rvl/GXStruct.h"
#include "rvl/GXTransform.h"
#include "rvl/GXGeometry.h"
#include "rvl/GXDispList.h"
#include "rvl/GXLighting.h"
#include "rvl/GXTev.h"
#include "rvl/GXTexture.h"
#include "rvl/GXCull.h"
#include "rvl/GXPixel.h"
#include "rvl/GXBump.h"
#include "rvl/GXVert.h"
#include "rvl/vifuncs.h"

class APDebugDrawer : public m3d::proc_c {
	public:
		APDebugDrawer();

		bool amISetUp;
		mHeapAllocator_c allocator;

		void setMeUp();

		void drawMe();

		void drawOpa();
		void drawXlu();
};


static APDebugDrawer defaultInstance;

int APDebugDraw() {
	defaultInstance.drawMe();
	return 1;
}


APDebugDrawer::APDebugDrawer() {
	amISetUp = false;
}

void APDebugDrawer::setMeUp() {
	allocator.setup(GameHeaps[0], 0x20);
	setup(&allocator);
}

void APDebugDrawer::drawMe() {
	if (!amISetUp) {
		setMeUp();
		amISetUp = true;
	}

	scheduleForDrawing();
}

void APDebugDrawer::drawOpa() {
}
void APDebugDrawer::drawXlu() {
	GXClearVtxDesc();

	GXSetVtxDesc(GX_VA_POS, GX_DIRECT);
	GXSetVtxDesc(GX_VA_CLR0, GX_DIRECT);

	GXSetVtxAttrFmt(GX_VTXFMT0, GX_VA_POS, GX_POS_XYZ, GX_F32, 0);
	GXSetVtxAttrFmt(GX_VTXFMT0, GX_VA_CLR0, GX_CLR_RGBA, GX_RGBA8, 0);

	GXSetNumIndStages(0);
	for (int i = 0; i < 0x10; i++)
		GXSetTevDirect((GXTevStageID)i);

	GXSetNumChans(1);
	GXSetChanCtrl(GX_COLOR0A0, GX_DISABLE, GX_SRC_REG, GX_SRC_VTX, GX_LIGHT_NULL, GX_DF_NONE, GX_AF_NONE);
	GXSetChanAmbColor(GX_COLOR0A0, (GXColor){255,255,255,255});
	GXSetChanMatColor(GX_COLOR0A0, (GXColor){255,255,255,255});
	GXSetNumTexGens(0);

	GXSetNumTevStages(1);
	GXSetNumIndStages(0);

	GXSetTevSwapModeTable(GX_TEV_SWAP0, GX_CH_RED, GX_CH_GREEN, GX_CH_BLUE, GX_CH_ALPHA);

	GXSetTevOrder(GX_TEVSTAGE0, GX_TEXCOORD_NULL, GX_TEXMAP_NULL, GX_COLOR0A0);
	GXSetTevOp(GX_TEVSTAGE0, GX_PASSCLR);
//	GXSetTevColorIn(GX_TEVSTAGE0, GX_CC_C1, GX_CC_C0, GX_CC_RASC, GX_CC_ZERO);
//	GXSetTevColorOp(GX_TEVSTAGE0, GX_TEV_ADD, GX_TB_ZERO, GX_CS_SCALE_1, GX_TRUE, GX_TEVPREV);
//	GXSetTevAlphaIn(GX_TEVSTAGE0, GX_CA_ZERO, GX_CA_A0, GX_CA_RASA, GX_CA_ZERO);
//	GXSetTevAlphaOp(GX_TEVSTAGE0, GX_TEV_ADD, GX_TB_ZERO, GX_CS_SCALE_1, GX_TRUE, GX_TEVPREV);

	GXSetZCompLoc(GX_FALSE);
	GXSetBlendMode(GX_BM_BLEND, GX_BL_SRCALPHA, GX_BL_INVSRCALPHA, GX_LO_SET);
	GXSetZMode(GX_TRUE, GX_LEQUAL, GX_FALSE);
	GXSetAlphaCompare(GX_GREATER, 0, GX_AOP_OR, GX_GREATER, 0);

	GXSetFog(GX_FOG_NONE, 0, 0, 0, 0, (GXColor){0,0,0,0});
	GXSetFogRangeAdj(GX_FALSE, 0, 0);

	GXSetCullMode(GX_CULL_NONE);

	GXSetDither(GX_TRUE);
	GXSetLineWidth(18, GX_TO_ZERO);

	GXSetTevColor(GX_TEVREG0, (GXColor){255,255,255,255});
	GXSetTevColor(GX_TEVREG1, (GXColor){0,0,0,255});

	ActivePhysics *ap = ActivePhysics::globalListHead;

	while (ap) {
		u32 uptr = (u32)ap;
		u8 r = (uptr>>16)&0xFF;
		u8 g = (uptr>>8)&0xFF;
		u8 b = uptr&0xFF;
		u8 a = 0xFF;

		GXBegin(GX_LINES, GX_VTXFMT0, 10);

		float centreX = ap->owner->pos.x + ap->info.xDistToCenter;
		float centreY = ap->owner->pos.y + ap->info.yDistToCenter;
		float edgeDistX = ap->info.xDistToEdge;
		float edgeDistY = ap->info.yDistToEdge;

		float tlX = centreX - edgeDistX, tlY = centreY + edgeDistY;
		float trX = centreX + edgeDistX, trY = centreY + edgeDistY;

		float blX = centreX - edgeDistX, blY = centreY - edgeDistY;
		float brX = centreX + edgeDistX, brY = centreY - edgeDistY;

		// Top
		GXPosition3f32(tlX, tlY, 8000.0f);
		GXColor4u8(r,g,b,a);
		GXPosition3f32(trX, trY, 8000.0f);
		GXColor4u8(r,g,b,a);

		// Left
		GXPosition3f32(tlX, tlY, 8000.0f);
		GXColor4u8(r,g,b,a);
		GXPosition3f32(blX, blY, 8000.0f);
		GXColor4u8(r,g,b,a);

		// Right
		GXPosition3f32(trX, trY, 8000.0f);
		GXColor4u8(r,g,b,a);
		GXPosition3f32(brX, brY, 8000.0f);
		GXColor4u8(r,g,b,a);

		// Bottom
		GXPosition3f32(blX, blY, 8000.0f);
		GXColor4u8(r,g,b,a);
		GXPosition3f32(brX, brY, 8000.0f);
		GXColor4u8(r,g,b,a);

		// Diagonal
		GXPosition3f32(trX, trY, 8000.0f);
		GXColor4u8(r,g,b,a);
		GXPosition3f32(blX, blY, 8000.0f);
		GXColor4u8(r,g,b,a);

		GXEnd();

		ap = ap->listPrev;
	}

	Physics *p = Physics::globalListHead;

	while (p) {
		u32 uptr = (u32)p;
		u8 r = (uptr>>16)&0xFF;
		u8 g = (uptr>>8)&0xFF;
		u8 b = uptr&0xFF;
		u8 a = 0xFF;

		GXBegin(GX_LINES, GX_VTXFMT0, 10);

		float centreX = p->owner->pos.x;
		float centreY = p->owner->pos.y;
		float edgeDistX = p->_88;
		float edgeDistY = p->_8C;

		float tlX = centreX + p->x, tlY = centreY + p->y;
		float trX = centreX + p->_88, trY = centreY + p->y;

		float blX = centreX + p->x, blY = centreY + p->_8C;
		float brX = centreX + p->_88, brY = centreY + p->_8C;

		// Top
		GXPosition3f32(tlX, tlY, 8000.0f);
		GXColor4u8(r,g,b,a);
		GXPosition3f32(trX, trY, 8000.0f);
		GXColor4u8(r,g,b,a);

		// Left
		GXPosition3f32(tlX, tlY, 8000.0f);
		GXColor4u8(r,g,b,a);
		GXPosition3f32(blX, blY, 8000.0f);
		GXColor4u8(r,g,b,a);

		// Right
		GXPosition3f32(trX, trY, 8000.0f);
		GXColor4u8(r,g,b,a);
		GXPosition3f32(brX, brY, 8000.0f);
		GXColor4u8(r,g,b,a);

		// Bottom
		GXPosition3f32(blX, blY, 8000.0f);
		GXColor4u8(r,g,b,a);
		GXPosition3f32(brX, brY, 8000.0f);
		GXColor4u8(r,g,b,a);

		// Diagonal
		GXPosition3f32(trX, trY, 8000.0f);
		GXColor4u8(r,g,b,a);
		GXPosition3f32(blX, blY, 8000.0f);
		GXColor4u8(r,g,b,a);

		GXEnd();

		p = p->next;
	}
}