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

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

PhysicsWorld::PhysicsWorld (Vector gravity, Vector dimensions)
    : tick_timer(PHYSICS_TICK_MS), gravity(gravity), dimensions(dimensions), terrain(dimensions.x, std::vector<TerrainType>(dimensions.y, EMPTY)) {

	generateTerrain(1337);

    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);
}

/**
 * Updates object speed and position. This function organises force
 * integration and collision detection.
 */   
void PhysicsObject::updatePosition () {
    // Add gravity to the force queue
    forceq.push(world.gravity);
    
    // Go trough every force in the queue
    // TODO: It might be possible to optimize by adding forces together
    Force total;
    posAfterTick = position;
    velAfterTick = velocity;
    while (!forceq.empty()) {
        total += forceq.front();
        forceq.pop();
        //        Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Current position: " << posAfterTick;
    }
    integrate(total, PHYSICS_TICK_MS);

    Vector newPosition = posAfterTick /*+ (velAfterTick * PHYSICS_TICK_MS)/1000*/;
    this->velocity = velAfterTick;
    //Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Nopeus: "<<this->velocity;
    /*
    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

    bool collided = false;

    //goes 1 unit forward every step and check if has hit anything
    Vector unitVector = (newPosition-position) / (newPosition-position).length();
    
	Vector tmpVector = position;
    Vector reached = position;
	int steps = (int) (newPosition-position).length();
    for(int i = 0; i < steps; i++) {
        tmpVector += unitVector;
        if(world.getType(tmpVector) != EMPTY) {
			//Engine::log(DEBUG, "physics.update_position") << "hit something";
            // Then we have hit something
            reached = position + unitVector*(i-1);
            collided = true;
            break;
        } else {
			//Engine::log(DEBUG, "physics.update_position") << "didnt hit";
		}
    }

    // In case of some float error
    if(!collided) {
        if(world.getType(newPosition)) {
            // There was error, and there is ground
            newPosition = tmpVector;
        } else {
            // This means everything was ok, so no need to do anything
        }
    } else {
        newPosition = reached;
        this->velocity = Vector(0, 0);
        //TODO: it shouldn't just stop on collision
    }
    this->position = newPosition;

}

bool PhysicsWorld::collided (Vector oldPos, Vector newPos) {
    int deltaX = oldPos.x - newPos.x; 
    int deltaY = oldPos.y - newPos.y; 
    double distance = sqrt(deltaX * deltaX + deltaY * deltaY);
    double xInc = deltaX / distance;
    double yInc = deltaY / distance;
    double currentX = oldPos.x;
    double currentY = oldPos.y;

    // This implementation is bit slow since it checks some squares twice.
    for(unsigned int i = 1; i < distance; i++) {
        currentX += xInc;
        currentY += yInc;
        if(terrain[(int)currentX][(int)currentY] != EMPTY)
            return true;
    }
    return false;
}

/**
 * Integrates given force over time and stores new position to
 * posAfterTick and new velocity to velAfterTick.
 * @param force Force vector.
 * @param dt The time the force is applied (<=PHYSICS_TICK_MS)
 */
void PhysicsObject::integrate(Force force, TimeMS dt) {
    Derivative tmpd;
    Derivative k1 = evaluate(force, 0, tmpd);
    Derivative k2 = evaluate(force, 0.5f*dt, k1);
    Derivative k3 = evaluate(force, 0.5f*dt, k2);
    Derivative k4 = evaluate(force, dt, k3);
    

    const Vector dxdt = (k1.dx + (k2.dx + k3.dx) * 2.0f + k4.dx) * 1.0f/6.0f;
    const Vector dvdt = (k1.dv + (k2.dv + k3.dv) * 2.0f + k4.dv) * 1.0f/6.0f;
    
    //    Engine::log(DEBUG, "PhysicsObject.integrate") << "Changes: "<< dxdt << " " << dvdt << " Time: " <<dt;
    posAfterTick = posAfterTick + (dxdt * dt)/1000;
    velAfterTick = velAfterTick + (dvdt * dt)/1000;
    //Engine::log(DEBUG, "PhysicsObject.integrate") << "velAfterTick: " << velAfterTick;
}

Derivative PhysicsObject::evaluate(Force force, TimeMS dt, Derivative &d) {
    Vector curPos = posAfterTick + (d.dx*dt)/1000;
    Vector curVel = velAfterTick + (d.dv*dt)/1000;

    Derivative out;
    out.dx = curVel;
    out.dv = acceleration(force);
    //Engine::log(DEBUG, "PhysicsObject.evaluate") << "Out.dx: " << out.dx;
    return out;
}

Vector PhysicsObject::acceleration(const Force &force) {
    return (force/mass);
}

/**
 * Adds force to the force queue. Force queue is emptied on each
 * tick. Forces that last over one tick are also handled.
 * @param force Force vector.
 * @param dt The time the force is applied.
 */
void PhysicsObject::applyForce (Force force, TimeMS dt) {
    // Add applied force to the queue
    forceq.push(force);
}

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

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

/**
 * simple random map generation
 * first fills whole level with dirt
 * then randomizes circles of empty or rock
 * @param seed - seed number for random number generator
 */
void PhysicsWorld::generateTerrain(int seed) {
    // generating should use own random number generator, but didn't find easily how that is done
    srand(seed);
    
    // some constants to control random generation
    const int min_range = 10;
    const int max_range = 40;
    const int num = 1;
    const int rock_rarity = 4; // 1 / rock_rarity will be rock circle

    // loops for amount of circles
    for(int i = 0; i < num; i++) {
        // information of new circle
        int midx = rand()%(int)dimensions.x;
        int midy = rand()%(int)dimensions.y;
        int range = rand()%(max_range-min_range)+min_range;

        // put first circle in the middle of the cave
		// so that we have some area we can certainly spawn into 
		if(i == 0) {
			midx = 60;
			midy = 60;
			range = 50;
		}

        TerrainType type = DIRT;
        if(rand()%rock_rarity == 0) {
            type = ROCK;
        }
        // loops for every pixel of circle
        for(int x = std::max(0, midx-range); x < std::min((int)dimensions.x, midx+range); x++) {
            for(int y = std::max(0, midy-range); y < std::min((int)dimensions.y, midy+range); y++) {
                if(x*x+y*y < range*range) {
                    // and sets it to type
                    terrain[x][y] = type;
                }
            }
        }
    }
}

/**
 * Returns terrainType in given tile. ROCK if tile is out of area
 * @param pos - coordinate of tile
 */
TerrainType PhysicsWorld::getType(Vector pos) const {
    int x = (int)(pos.x);
    int y = (int)(pos.y);
    if(x < 0 || y < 0 || x >= dimensions.x || y >= dimensions.y) {
        return ROCK;
    }
    return terrain[x][y];
}