#include "PhysicsWorld.hh"
#include "Engine.hh"

#include <functional>

PhysicsWorld::PhysicsWorld (Vector gravity, Vector dimensions) :
    // XXX: assume Vector == PixelCoordinate
    Terrain((unsigned int) dimensions.x, (unsigned int) dimensions.y, 1337), 
    dimensions(dimensions), 
    gravity(gravity),
    tick_timer(PHYSICS_TICK_MS)
{
    slots.connect(tick_timer.sig_tick(), this, &PhysicsWorld::tick);
    tick_timer.start();
}

void PhysicsWorld::addPhysicsObject (PhysicsObject *po) {
    objects.push_back(po);
}

void PhysicsWorld::removePhysicsObject (PhysicsObject *po) {
    objects.remove(po);
}


float distancePointToLine(Vector l1, Vector l2, Vector p) {
    Vector v(l2.y - l1.y, -(l2.x - l1.x));
    Vector r(l1.x-p.x, l1.y-p.y);
    v = v/v.length();
    float dist = v*r/v.sqrLength();
    return dist;
}

void PhysicsWorld::tick (TimeMS tick_length) {
    // tick each object in turn
    for (std::list<PhysicsObject*>::iterator i = objects.begin(); i != objects.end(); i++) {
        (*i)->tick(tick_length); 
    }
    for (std::list<PhysicsObject*>::iterator i = objects.begin(); i != objects.end(); i++) {
        for (std::list<PhysicsObject*>::iterator j = i; j != objects.end(); j++) {
            if(i == j)
                continue;
            float range_sum_sqr = 2;
            if((*i)->getType() == PLAYER) {
                range_sum_sqr += PLAYER_RADIUS-1;
            }
            if((*j)->getType() == PLAYER) {
                range_sum_sqr += PLAYER_RADIUS-1;
            }
            range_sum_sqr *= range_sum_sqr;
            bool collision = false;
            Vector a1 = (*i)->getPreviousPosition();
            Vector a2 = (*i)->getPosition();
            Vector b1 = (*j)->getPreviousPosition();
            Vector b2 = (*j)->getPosition();
/*            if(a1 == a2) {
                float d = abs(distancePointToLine(b1, b2, a1));
                if(d*d < range_sum_sqr)
                    collision = true;
            } else if(b1 == b2) {
                float d = abs(distancePointToLine(a1, a2, b1));
                if(d*d < range_sum_sqr)
                    collision = true;
            } else {*/
                float db1 = distancePointToLine(a1, a2, b1);
                float db2 = distancePointToLine(a1, a2, b2);
                float da1 = distancePointToLine(b1, b2, a1);
                float da2 = distancePointToLine(b1, b2, a2);
                if(db1*db2 < 0 && da1*da2 < 0) {
                    // lines intersected
//                    collision = true;
                }
                float dab = (a2-b2).sqrLength();
                if(dab < range_sum_sqr) {
                    collision = true;
                }
//            }
            if(collision) {
                (*i)->onCollision(a2, *j);
                (*j)->onCollision(b2, *i);
            }
        }
    }

    // Delete destroyed objects
    objects.remove_if(std::mem_fun(&PhysicsObject::removeIfDestroyed));
}

TickCount PhysicsWorld::getTicks (void) {
    return tick_timer.get_ticks();
}