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

#include <algorithm>
#include <functional>
#include <cmath>

PhysicsWorld::PhysicsWorld (Vector gravity, Vector dimensions, Terrain &terrain) :
    terrain(terrain),
    dimensions(dimensions), 
    gravity(gravity),
    tick_timer(PHYSICS_TICK_MS)
{
    // wire up our timer
    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); 
    }
    // Object - object collision detection
    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 a2 = (*i)->getPosition();
            Vector b2 = (*j)->getPosition();
            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();
}