#include "ofxBox2dRender.h"
#include "ofxBox2dUtils.h"

void ofxBox2dDebugDraw::DrawPolygon(const b2Vec2* _vertices, int32 vertexCount, const b2Color& color) {
    b2Vec2 vertices;

	glColor3f(color.r, color.g, color.b);
	glBegin(GL_LINE_LOOP);
	for (int32 i = 0; i < vertexCount; ++i) {
        vertices = _vertices[i];
        vertices *= OFX_BOX2D_SCALE;
		glVertex2f(vertices.x, vertices.y);
	}
	glEnd();
}

void ofxBox2dDebugDraw::DrawSolidPolygon(const b2Vec2* _vertices, int32 vertexCount, const b2Color& color) {
    b2Vec2 vertices;

	glEnable(GL_BLEND);
	glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	glColor4f(0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 0.5f);
	glBegin(GL_TRIANGLE_FAN);
	for (int32 i = 0; i < vertexCount; ++i) {
        vertices = _vertices[i];
        vertices *= OFX_BOX2D_SCALE;
		glVertex2f(vertices.x, vertices.y);
	}
	glEnd();
	glDisable(GL_BLEND);

	glColor4f(color.r, color.g, color.b, 1.0f);
	glBegin(GL_LINE_LOOP);
	for (int32 i = 0; i < vertexCount; ++i) {
        vertices = _vertices[i];
        vertices *= OFX_BOX2D_SCALE;
		glVertex2f(vertices.x, vertices.y);
	}
	glEnd();
}

void ofxBox2dDebugDraw::DrawCircle(const b2Vec2& _center, float32 radius, const b2Color& color) {
    b2Vec2 center = _center;
    center *= OFX_BOX2D_SCALE;
    radius *= OFX_BOX2D_SCALE;

	const float32 k_segments = 16.0f;
	const float32 k_increment = 2.0f * b2_pi / k_segments;
	float32 theta = 0.0f;
	glColor3f(color.r, color.g, color.b);
	glBegin(GL_LINE_LOOP);
	for (int32 i = 0; i < k_segments; ++i) {
		b2Vec2 v = center + radius * b2Vec2(cosf(theta), sinf(theta));
		glVertex2f(v.x, v.y);
		theta += k_increment;
	}
	glEnd();
}

void ofxBox2dDebugDraw::DrawSolidCircle(const b2Vec2& _center, float32 radius, const b2Vec2& axis, const b2Color& color) {
    b2Vec2 center = _center;
    center *= OFX_BOX2D_SCALE;
    radius *= OFX_BOX2D_SCALE;

	const float32 k_segments = 16.0f;
	const float32 k_increment = 2.0f * b2_pi / k_segments;
	float32 theta = 0.0f;
	glEnable(GL_BLEND);
	glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	glColor4f(0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 0.5f);
	glBegin(GL_TRIANGLE_FAN);
	for (int32 i = 0; i < k_segments; ++i) {
		b2Vec2 v = center + radius * b2Vec2(cosf(theta), sinf(theta));
		glVertex2f(v.x, v.y);
		theta += k_increment;
	}
	glEnd();
	glDisable(GL_BLEND);

	theta = 0.0f;
	glColor4f(color.r, color.g, color.b, 1.0f);
	glBegin(GL_LINE_LOOP);
	for (int32 i = 0; i < k_segments; ++i) {
		b2Vec2 v = center + radius * b2Vec2(cosf(theta), sinf(theta));
		glVertex2f(v.x, v.y);
		theta += k_increment;
	}
	glEnd();

	b2Vec2 p = center + radius * axis;
	glBegin(GL_LINES);
	glVertex2f(center.x, center.y);
	glVertex2f(p.x, p.y);
	glEnd();
}

void ofxBox2dDebugDraw::DrawSegment(const b2Vec2& _p1, const b2Vec2& _p2, const b2Color& color) {
    b2Vec2 p1 = _p1;
    b2Vec2 p2 = _p2;

    p1 *= OFX_BOX2D_SCALE;
    p2 *= OFX_BOX2D_SCALE;

	glColor3f(color.r, color.g, color.b);
	glBegin(GL_LINES);
	glVertex2f(p1.x, p1.y);
	glVertex2f(p2.x, p2.y);
	glEnd();
}

void ofxBox2dDebugDraw::DrawTransform(const b2Transform& xf) {
	b2Vec2 p1 = xf.position, p2;
	p1 *= OFX_BOX2D_SCALE;

	const float32 k_axisScale = 0.4f;
	glBegin(GL_LINES);

	glColor3f(1.0f, 0.0f, 0.0f);
	glVertex2f(p1.x, p1.y);
	p2 = p1 + k_axisScale * xf.R.col1;
	glVertex2f(p2.x, p2.y);

	glColor3f(0.0f, 1.0f, 0.0f);
	glVertex2f(p1.x, p1.y);
	p2 = p1 + k_axisScale * xf.R.col2;
	glVertex2f(p2.x, p2.y);

	glEnd();
}

void ofxBox2dDebugDraw::DrawPoint(const b2Vec2& _p, float32 size, const b2Color& color) {
    b2Vec2 p = _p;
    p *= OFX_BOX2D_SCALE;

	glPointSize(size);
	glBegin(GL_POINTS);
	glColor3f(color.r, color.g, color.b);
	glVertex2f(p.x, p.y);
	glEnd();
	glPointSize(1.0f);
}

void ofxBox2dDebugDraw::DrawString(int x, int y, const char *string, ...) {
	char buffer[128];

	va_list arg;
	va_start(arg, string);
	vsprintf(buffer, string, arg);
	va_end(arg);

	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();
	int w = glutGet(GLUT_WINDOW_WIDTH);
	int h = glutGet(GLUT_WINDOW_HEIGHT);
	gluOrtho2D(0, w, h, 0);
	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	glLoadIdentity();

	glColor3f(0.9f, 0.6f, 0.6f);
	glRasterPos2i(x, y);
	int32 length = (int32)strlen(buffer);
	for (int32 i = 0; i < length; ++i) {
		glutBitmapCharacter(GLUT_BITMAP_8_BY_13, buffer[i]);
	}

	glPopMatrix();
	glMatrixMode(GL_PROJECTION);
	glPopMatrix();
	glMatrixMode(GL_MODELVIEW);
}

void ofxBox2dDebugDraw::DrawAABB(b2AABB* aabb, const b2Color& c) {
	glColor3f(c.r, c.g, c.b);
	glBegin(GL_LINE_LOOP);
	glVertex2f(aabb->lowerBound.x * OFX_BOX2D_SCALE, aabb->lowerBound.y * OFX_BOX2D_SCALE);
	glVertex2f(aabb->upperBound.x * OFX_BOX2D_SCALE, aabb->lowerBound.y * OFX_BOX2D_SCALE);
	glVertex2f(aabb->upperBound.x * OFX_BOX2D_SCALE, aabb->upperBound.y * OFX_BOX2D_SCALE);
	glVertex2f(aabb->lowerBound.x * OFX_BOX2D_SCALE, aabb->upperBound.y * OFX_BOX2D_SCALE);
	glEnd();
}
