#include "Physics.hh"
#include "Engine.hh"

#include <algorithm>
#include <functional>

PhysicsWorld::PhysicsWorld (Vector gravity, Vector dimensions)
    : tick_timer(PHYSICS_TICK_MS), gravity(gravity), dimensions(dimensions) {

    slots.connect(tick_timer.sig_timer(), this, &PhysicsWorld::tick);
    tick_timer.enable();
}

void PhysicsWorld::addObject (PhysicsObject *object) {
    objects.push_back(object);
}

void PhysicsWorld::tick () {
//    Engine::log(DEBUG, "physics.apply_force") << "*tick*";

	for (std::vector<PhysicsObject*>::iterator i = objects.begin(); i != objects.end(); i++) {
       	(*i)->tick(); 
  	}
}

PhysicsObject::PhysicsObject (PhysicsWorld &world, float mass, Vector position, Vector velocity)
    : world(world), mass(mass), position(position), velocity(velocity) {

    world.addObject(this);
}
    
void PhysicsObject::updatePosition () {

	// Check if the player is moving on the ground
	/*if (this->velocity.y == 0 && (position.y >= world.dimensions.y - 3)) {
    	position.x += 50 * velocity.x * (PHYSICS_TICK_MS / 1000.0);
		velocity.x = 0;
		return;
	}*/

	// If not moving on the ground, apply normal physics

    // Calculate gravity's influence on the velocity vector
    this->velocity += world.gravity * (PHYSICS_TICK_MS / 1000.0);
        
    Vector newPosition = position + velocity * (PHYSICS_TICK_MS / 1000.0);

    //TODO Handle the object as a square or a polygon
    
//    Engine::log(DEBUG, "physics.update_position") << "position=" << newPosition << ", velocity=" << velocity;

    bool collided = false;
     
    if (newPosition.x < 0 || (newPosition.x > world.dimensions.x)) {
        // CRASH!
        this->velocity.x *= -0.5;
		
		// If the velocity drops under some fixed constant we decide it is zero.
		// This is to prevent the object from bouncing eternally.
		if (abs(this->velocity.x) < 0.1)
			this->velocity.x = 0;

        collided = true;
    } else {
        this->position.x = newPosition.x;
    }
    
    if (newPosition.y <= 0 || (newPosition.y >= world.dimensions.y)) {
		this->velocity.y *= -0.3;

		
 
		if (abs(this->velocity.y) < 0.1) {
			this->velocity.y = 0;
			// Friction
			this->velocity.x *= 0.95;
		} else {
        	// Bigger friction
			this->velocity.x *= 0.75;
		}

        collided = true;
	} else {
        this->position.y = newPosition.y;
	}
    
    if(!collided) {
        this->position = newPosition;
    }
}

void PhysicsObject::applyForce (Vector force, uint16_t dt) {
    Vector oldVelocity = velocity;

    this->velocity += force * dt / 1000 / mass;  // The last factor denotes the time.
    // It should be scaled somehow.
    
//    Engine::log(DEBUG, "physics.apply_force") << "force=" << force << ", velocity " << oldVelocity << " -> " << velocity;
}

void PhysicsObject::updatePhysics (Vector position, Vector velocity) {
    this->position = position;
    this->velocity = velocity;
}
    
Vector PhysicsObject::getPosition () {
    return this->position;
}

void PhysicsObject::tick () {
    this->updatePosition();
}