#include "Physics.hh"
#include "Engine.hh"
#include "GameState.hh"
#include "Terrain.hh"

#include <algorithm>
#include <functional>
#include <cmath>
#include <assert.h>

PhysicsWorld::PhysicsWorld (Vector gravity, Vector dimensions)
    : tick_timer(PHYSICS_TICK_MS), tick_counter(0), dimensions(dimensions),
      gravity(gravity) {
    terrain = Terrain(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(); 
    }

    tick_counter++;
}

uint32_t PhysicsWorld::getTick (void) {
    return tick_counter;
}

PhysicsObject::PhysicsObject (PhysicsWorld &world, float mass, 
                              Vector position, Vector velocity)
    : world(world), position(position), velocity(velocity),
      mass(mass), inAir(true), aim(0), facingRight(true) {
    // TODO: Is thir the right way to do this?
    world.addObject(this);
}

Vector PhysicsObject::walk (bool right) {
    Vector cursor = right ? this->position + Vector(1,0) : 
        this->position + Vector(-1,0);
    Vector reached = this->position;
    
    //for(int steps = 0; steps < 3; steps++) {
    
    // Go up but not if the wall is over two pixels
    if(world.getType(cursor) != EMPTY) {
        for(int height = 0, max = 3; height < max+42; height++) {
            
            if(height >= max)
                return reached;
            
            cursor.y--;
            if(world.getType(cursor) == EMPTY) {
                // Check that the other parts of the worm don't collide with anything
                if(possibleLocation(cursor)) {
                    reached = cursor;
                    continue;
                } else {
                    // Can't get any further
                    return reached;
                }
            }
        }
    } else {
        if(possibleLocation(cursor)) {
            reached = cursor;
        }
        // Start checking if the lower squares are empty
        
        for(int depth = 0, max = 3; depth < max+42; depth++) {
            
            if(depth >= max) {
                // We can start a free fall now
                //
                // TODO it should be set inAir if it falls from a cliff
                this->inAir = true;
                
                // Put some speed there to make loke smoother
                //this->velocity.y = -5;
                return reached;
            }
            
            cursor.y++;
            if(world.getType(cursor) == EMPTY) {
                // Check that the other parts of the worm don't collide with anything
                if(possibleLocation(cursor)) {
                    reached = cursor;
                    continue;
                } else {
                    // Can't get any further
                    return reached;
                }
            }
        }
    }      
    
    //    cursor.x += right ? 1 : -1;
    //}
    return reached;
}

void PhysicsObject::jump () {
    // Jump only if player is "on the ground"
    if (!this->inAir) {
 	    velocity.y = -100;
        velocity.x += facingRight ? 20 : -20;
	    inAir = true;
    }
}

bool PhysicsObject::possibleLocation (Vector loc) {
    for(unsigned int i = 0; i < this->shape.size(); i++) {
        if(world.getType(loc+shape[i]) != EMPTY)
            return false;
    }
    return true;
}

/**
 * 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
    Force total;
    posAfterTick = position;
    velAfterTick = velocity;
    while (!forceq.empty()) {
        total += forceq.front();
        forceq.pop();
    }

    // TODO: This is _ugly_ (but not so ugly as the last one I think)
    // hack. I think we should handle walking from the collision
    // detection code.

    if (!this->inAir) {
        if (total.x != 0)
            this->position = walk(total.x > 0);
        return; 
        //total.x = 0;
    }

    integrate(total, PHYSICS_TICK_MS);

    Vector newPosition = posAfterTick /*+ (velAfterTick * PHYSICS_TICK_MS)/1000*/;
    this->velocity = velAfterTick;

   
    // Collision detection
    bool collided = false;
   
    const Vector diffVec = newPosition-position;
    const Vector unitVector = diffVec / diffVec.length();
    Vector reached = position;

    while ((position-reached).length() < diffVec.length()) {
        // Check if any of the shapes points collide
        for (uint64_t i = 0; i < shape.size(); i++) {
            if (world.getType(reached+shape[i]) != EMPTY) {  // Collision
                if (inAir) {
                    //                    Engine::log(DEBUG, "Here");
                    this->bounce(world.getNormal(reached+shape[i], 
                                                 reached-unitVector+shape[i]));
                    //this->velocity *= COLLISION_ELASTICITY;
                }
                reached = reached - unitVector; // Return to last point
                collided = true;
                if (this->velocity.length() < PLAYER_MIN_SPEED) {
                    this->inAir = false;
                    this->velocity = Vector(0,0);
                }
                break;
            }
        }
        if (collided)
            break;
        reached += unitVector;
    }
   
    
    /*
      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() + 2;
        
      //Engine::log(DEBUG, "physics.update_position") << unitVector-newPosition;
      //Vector foo = position+unitVector*steps-newPosition;
      //Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Virhe: "<< foo;
    
      for(int i = 0; i < steps; i++) {
      tmpVector += unitVector;

      float minVelocity = 10;
      // Check if any of the four corners of the worm collide
      for(int sh = 0; sh < 4; sh++) {
      if(world.getType(tmpVector+shape[sh]) != EMPTY) {
      reached = position + unitVector*(i-1);
      collided = true;
      this->bounce(world.getNormal(tmpVector+shape[sh], tmpVector-unitVector+shape[sh]));
      this->velocity *= 0.4;
      if(abs(this->velocity.x) < minVelocity && (abs(this->velocity.y) < minVelocity)) {
      this->inAir = false;
      this->velocity = Vector(0,0);
      }
      break;
      }
      }
      if(collided)
      break;
      }
    */

    // In case of some float error check the final coordinate
    if(!collided) {
        if(world.getType(newPosition+shape[0]) != EMPTY || (world.getType(newPosition+shape[1]) != EMPTY) 
           || (world.getType(newPosition+shape[2]) != EMPTY) || (world.getType(newPosition+shape[3]) != EMPTY)) {
            
            //  Engine::log(DEBUG, "physics.update_position") << "didnt hit";
            // There was error, and there is ground
            //newPosition = tmpVector;
        } else {
            // This means everything was ok, so no need to do anything
        }
    } else {
        newPosition = reached;
        onCollision();
        //this->velocity = Vector(0, 0);
        //TODO: it shouldn't just stop on collision
    }
    this->position = newPosition;
//    Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Pos: " << this->position;
}

/**
 * Gets the index of the given coordinate direction
 * referring to the DIRECTIONS table in Physics.hh
 *
 * moved to Terrain.cc
 *//*
int getDirectionIndex (Vector dir) {
    if(dir.x == 0 && dir.y == -1) {
        return 0;
    } else if(dir.x == 1 && dir.y == -1) {
        return 1;
    } else if(dir.x == 1 && dir.y == 0) {
        return 2;
    } else if(dir.x == 1 && dir.y == 1) {
        return 3;
    } else if(dir.x == 0 && dir.y == 1) {
        return 4;
    } else if(dir.x == -1 && dir.y == 1) {
        return 5;
    } else if(dir.x == -1 && dir.y == 0) {
        return 6;
    } else if(dir.x == -1 && dir.y == -1) {
        return 7;
    }
    Engine::log(DEBUG, "physics.getDirectionIndex ") << "invalid direction: " << dir;
    return 0;
}*/

/**
 * Computes hitten wall's normal. Calculated from 3*3 grid
 */
Vector PhysicsWorld::getNormal (Vector hitPoint, Vector prevPoint) {
    return terrain.getNormal(hitPoint, prevPoint);
}

/**
 * Bounces from straight wall in any direction.
 * Direction given as normal of that wall
 */
void PhysicsObject::bounce (Vector normal) {
    if (normal.length() != 0) {
        // Engine::log(DEBUG, "PhysicsObject.bounce") << "Velocity: " << velocity;
        Vector nvel = velocity;
        //        Engine::log(DEBUG, "PhysicsObject.bounce") << "New Velocity: " << nvel;
        nvel = nvel - (2*((nvel*normal)/(normal*normal))*normal);
        //        Engine::log(DEBUG, "PhysicsObject.bounce") << "Projection: " << nvel;
        velocity = nvel;
        this->velocity *= COLLISION_ELASTICITY;
    }
}

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

void PhysicsObject::applyForce (Force force) {
    // Add applied force to the queue
    forceq.push(force);
}

void PhysicsObject::changeAim(float da) {
    this->aim += da;

    if (this->aim > PLAYER_AIM_MAX) this->aim = PLAYER_AIM_MAX;
    if (this->aim < PLAYER_AIM_MIN) this->aim = PLAYER_AIM_MIN;
    //Engine::log(DEBUG, "PhysicsObject.changeAim") << "Player aim: " << this->aim;
}

void PhysicsObject::setFacing(bool facingRight) {
    //Engine::log(DEBUG, "PhysicsObject.setFacing") << "Facing: " << right;
    this->facingRight = facingRight;
}

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

bool PhysicsObject::getFacing() {
    return this->facingRight;
}

float PhysicsObject::getAim() {
    return this->aim;
}

std::vector<Vector>& PhysicsObject::getShape () {
    return this->shape;
}

void PhysicsObject::setShape (std::vector<Vector> shape) {
    this->shape = shape;
}

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

void PhysicsObject::draw(CL_GraphicContext *gc) {
    CL_Quad player(
                   (position+shape[0]).x, (position+shape[0]).y,
                   (position+shape[1]).x, (position+shape[1]).y,
                   (position+shape[2]).x, (position+shape[2]).y,
                   (position+shape[3]).x, (position+shape[3]).y
                   );
    
    gc->fill_quad(player, CL_Color::green);
    
    const uint16_t chlen = 10;
    uint16_t x = player.center().x;
    uint16_t y = player.center().y;
    if (facingRight) {
        gc->draw_line(x, y,
                      x + std::cos(aim)*chlen,
                      y - std::sin(aim)*chlen,
                      CL_Color::black);
    } else {
        gc->draw_line(x, y,
                      x - std::cos(aim)*chlen,
                      y - std::sin(aim)*chlen,
                      CL_Color::black);
    }
}

/**
 * Returns terrainType in given tile. ROCK if tile is out of area
 * @param pos - coordinate of tile
 */
TerrainType PhysicsWorld::getType(int x, int y) const {
    return terrain.getType((int32_t)x,(int32_t)y);
}
TerrainType PhysicsWorld::getType(Vector pos) const {
    return terrain.getType(pos.x, pos.y);
}

/**
 * Removes ground from given circle. ROCK is not removed.
 * @param (x, y) or pos - center of circle.
 * @param r - radius of circle
 */
void PhysicsWorld::removeGround(int x, int y, float r) {
    terrain.removeGround(Vector(x,y), r);
}
void PhysicsWorld::removeGround(Vector pos, float r) {
    terrain.removeGround(pos, r);
}

void PhysicsWorld::drawTerrain(CL_GraphicContext *gc) {
    terrain.draw(gc);
}