#include "ofxBox2d.h"

ofxBox2d::ofxBox2d() {
    pause = false;
    singleStep = false;

    drawStats = false;
    drawShapes = true;
    drawJoints = true;
    drawAABBs = false;
    drawPairs = false;
    drawCOMs = false;

    enableWarmStarting = false;
    enableContinuous = true;

    fps = ofGetFrameRate();

    velocityIterations = 8;
    positionIterations = 3;

    gravity.Set(0.0f, 9.81f);
	doSleep = true;
}

ofxBox2d::~ofxBox2d() {
	delete m_world;
	m_world = NULL;
}

void ofxBox2d::setup() {
	m_world = new b2World(gravity, doSleep);
	m_mouseJoint = NULL;

	m_world->SetDestructionListener(this);
	m_world->SetContactListener(this);
	m_world->SetDebugDraw(&m_debugDraw);

	b2BodyDef groundBodyDef;
	m_groundBody = m_world->CreateBody(&groundBodyDef);
}

void ofxBox2d::update() {
    if (!m_world) {
        return;
    }

//	float timeStep = fps > 0.0f ? 1.0f / fps : float(0.0f);
	float timeStep = ofGetLastFrameTime();

	if (pause) {
		if (singleStep) {
			singleStep = false;
		}
		else {
			timeStep = 0.0f;
		}
	}

	uint32 flags = 0;
	flags += (drawShapes ? 1 : 0)			* b2DebugDraw::e_shapeBit;
	flags += (drawJoints ? 1 : 0)			* b2DebugDraw::e_jointBit;
	flags += (drawAABBs ? 1 : 0)			* b2DebugDraw::e_aabbBit;
	flags += (drawPairs ? 1 : 0)			* b2DebugDraw::e_pairBit;
	flags += (drawCOMs ? 1 : 0)				* b2DebugDraw::e_centerOfMassBit;
	m_debugDraw.SetFlags(flags);

	m_world->SetWarmStarting(enableWarmStarting);
	m_world->SetContinuousPhysics(enableContinuous);

	m_world->Step(timeStep, velocityIterations, positionIterations);
}

void ofxBox2d::draw() {
    if (!m_world) {
        return;
    }

    m_world->DrawDebugData();

	if (m_mouseJoint)
	{
		b2Vec2 p1 = m_mouseJoint->GetAnchorB();
		b2Vec2 p2 = m_mouseJoint->GetTarget();

		p1 *= OFX_BOX2D_SCALE;
		p2 *= OFX_BOX2D_SCALE;

		glPointSize(4.0f);
		glColor3f(0.0f, 1.0f, 0.0f);
		glBegin(GL_POINTS);
		glVertex2f(p1.x, p1.y);
		glVertex2f(p2.x, p2.y);
		glEnd();
		glPointSize(1.0f);

		glColor3f(0.8f, 0.8f, 0.8f);
		glBegin(GL_LINES);
		glVertex2f(p1.x, p1.y);
		glVertex2f(p2.x, p2.y);
		glEnd();
	}

    m_debugDraw.DrawString(2, 12, "bodies/contacts/pairs/joints/proxies = %d/%d/%d/%d",
        m_world->GetBodyCount(), m_world->GetContactCount(), m_world->GetJointCount(), m_world->GetProxyCount());
}

void ofxBox2d::setBounds(float x, float y, float w, float h, bool softEdges) {
    if (!m_world) {
        return;
    }

    if (m_groundBody) {
        m_world->DestroyBody(m_groundBody);
        b2BodyDef groundBodyDef;
        m_groundBody = m_world->CreateBody(&groundBodyDef);
    }

    b2FixtureDef fd;
    fd.friction = 0.6f;
    fd.restitution = 0.0f;

    if (softEdges) {
        b2PolygonShape groundBox;

        fd.shape = &groundBox;

        groundBox.SetAsEdge(b2Vec2(x/OFX_BOX2D_SCALE,y/OFX_BOX2D_SCALE), b2Vec2((x/OFX_BOX2D_SCALE) + (w/OFX_BOX2D_SCALE), y/OFX_BOX2D_SCALE));
        m_groundBody->CreateFixture(&fd);

        groundBox.SetAsEdge(b2Vec2(x/OFX_BOX2D_SCALE,y/OFX_BOX2D_SCALE), b2Vec2(x/OFX_BOX2D_SCALE, (y/OFX_BOX2D_SCALE) + (h/OFX_BOX2D_SCALE)));
        m_groundBody->CreateFixture(&fd);

        groundBox.SetAsEdge(b2Vec2(x/OFX_BOX2D_SCALE, (y/OFX_BOX2D_SCALE) + (h/OFX_BOX2D_SCALE)), b2Vec2((x/OFX_BOX2D_SCALE) + (w/OFX_BOX2D_SCALE), (y/OFX_BOX2D_SCALE) + (h/OFX_BOX2D_SCALE)));
        m_groundBody->CreateFixture(&fd);

        groundBox.SetAsEdge(b2Vec2((x/OFX_BOX2D_SCALE) + (w/OFX_BOX2D_SCALE), (y/OFX_BOX2D_SCALE) + (h/OFX_BOX2D_SCALE)), b2Vec2((x/OFX_BOX2D_SCALE) + (w/OFX_BOX2D_SCALE), y/OFX_BOX2D_SCALE));
        m_groundBody->CreateFixture(&fd);
    }
    else {
        if (m_body) {
            m_world->DestroyBody(m_groundBody);
        }

        b2BodyDef bodyDef;
        m_body = m_world->CreateBody(&bodyDef);

        m_bounds[0].Set(x/OFX_BOX2D_SCALE,y/OFX_BOX2D_SCALE);
        m_bounds[1].Set(x/OFX_BOX2D_SCALE+w/OFX_BOX2D_SCALE,y/OFX_BOX2D_SCALE);
        m_bounds[2].Set(x/OFX_BOX2D_SCALE+w/OFX_BOX2D_SCALE,y/OFX_BOX2D_SCALE+h/OFX_BOX2D_SCALE);
        m_bounds[3].Set(x/OFX_BOX2D_SCALE,y/OFX_BOX2D_SCALE+h/OFX_BOX2D_SCALE);

        b2LoopShape loop;
        loop.m_vertices = m_bounds;
        loop.m_count = 4;

        fd.shape = &loop;

        m_body->CreateFixture(&fd);
    }
}

void ofxBox2d::setFrameRate(int targetRate) {
    fps = targetRate;
    ofSetFrameRate(targetRate);
}

void ofxBox2d::registerGrabbing() {
	ofAddListener(ofEvents.mousePressed, this, &ofxBox2d::MouseDown);
	ofAddListener(ofEvents.mouseDragged, this, &ofxBox2d::MouseMove);
	ofAddListener(ofEvents.mouseReleased, this, &ofxBox2d::MouseUp);
}

void ofxBox2d::unregisterGrabbing() {
	ofRemoveListener(ofEvents.mousePressed, this, &ofxBox2d::MouseDown);
	ofRemoveListener(ofEvents.mouseDragged, this, &ofxBox2d::MouseMove);
	ofRemoveListener(ofEvents.mouseReleased, this, &ofxBox2d::MouseUp);
}

void ofxBox2d::MouseDown(ofMouseEventArgs &e) {
    if (!m_world) {
        return;
    }

    b2Vec2 p(e.x / OFX_BOX2D_SCALE, e.y / OFX_BOX2D_SCALE);
	m_mouseWorld = p;

	if (m_mouseJoint != NULL) {
		return;
	}

	// Make a small box.
	b2AABB aabb;
	b2Vec2 d;
	d.Set(0.001f, 0.001f);
	aabb.lowerBound = p - d;
	aabb.upperBound = p + d;

	// Query the world for overlapping shapes.
	QueryCallback callback(p);
	m_world->QueryAABB(&callback, aabb);

	if (callback.m_fixture) {
		b2Body* body = callback.m_fixture->GetBody();
		b2MouseJointDef md;
		md.bodyA = m_groundBody;
		md.bodyB = body;
		md.target = p;
		md.maxForce = 1000.0f * body->GetMass();
		m_mouseJoint = (b2MouseJoint*)m_world->CreateJoint(&md);
		body->SetAwake(true);
	}
}

void ofxBox2d::MouseUp(ofMouseEventArgs &e) {
    if (!m_world) {
        return;
    }

	if (m_mouseJoint) {
		m_world->DestroyJoint(m_mouseJoint);
		m_mouseJoint = NULL;
	}
}

void ofxBox2d::MouseMove(ofMouseEventArgs &e) {
    if (!m_world) {
        return;
    }

    b2Vec2 p(e.x / OFX_BOX2D_SCALE, e.y / OFX_BOX2D_SCALE);
	m_mouseWorld = p;

	if (m_mouseJoint) {
		m_mouseJoint->SetTarget(p);
	}
}

// b2ContactListener
void ofxBox2d::BeginContact(b2Contact* contact) {}
void ofxBox2d::EndContact(b2Contact* contact) {}
void ofxBox2d::PreSolve(b2Contact* contact, const b2Manifold* oldManifold) {}
void ofxBox2d::PostSolve(const b2Contact* contact, const b2ContactImpulse* impulse) {}

// b2DestructionListener
void ofxBox2d::SayGoodbye(b2Fixture* fixture) { }

void ofxBox2d::SayGoodbye(b2Joint* joint) {
	if (m_mouseJoint == joint) {
        m_mouseJoint = NULL;
	}
}


QueryCallback::QueryCallback(const b2Vec2& point) {
    m_point = point;
    m_fixture = NULL;
}

bool QueryCallback::ReportFixture(b2Fixture* fixture) {
    b2Body* body = fixture->GetBody();
    if (body->GetType() == b2_dynamicBody) {
        bool inside = fixture->TestPoint(m_point);
        if (inside) {
            m_fixture = fixture;
            return false;
        }
    }
    return true;
}
