#include "gameplay/physics.h"
#include "gameplay/weapons/bullet.h"
#include "utility/events.h"

void PhysicsEngine::hookEventManager(
    const std::shared_ptr<EventManager> &eventManager) {
  this->eventManager = eventManager;
  this->eventManager->subscribe<BulletFiredEvent>(
      [this](const BulletFiredEvent &event) {
        if (auto bullet = event.bullet.lock()) {
          this->addObject(bullet->getPhysicsComponent());
        }
      });
  this->eventManager->subscribe<BulletDiedEvent>(
      [this](const BulletDiedEvent &event) {
        this->removeObject(event.physObj);
      });
}

std::shared_ptr<PhysicsComponent>
PhysicsEngine::createObject(const unsigned int ID, const glm::vec3 &pos,
                            float mass, PhysicsComponent::Collider::Shape shape,
                            glm::vec3 dimensions, const glm::vec3 offset) {
  auto component = std::make_shared<PhysicsComponent>();
  component->ID = ID;
  component->rigidBody.position = pos;
  component->rigidBody.acceleration = glm::vec2(0.f);
  component->rigidBody.mass = mass;
  component->collider.shape = shape;
  component->collider.dimensions = dimensions;
  component->collider.offset = offset;

  addObject(component);
  return component;
}

void PhysicsEngine::loadCollisionMap(
    const std::vector<std::vector<int>> &collisionMap, float tileSize) {
  this->collisionMap = collisionMap;
  this->tileSize = tileSize;
}

void PhysicsEngine::addObject(
    const std::shared_ptr<PhysicsComponent> &component) {
  if (component)
    objects.emplace_back(component);
}

void PhysicsEngine::removeObject(
    const std::shared_ptr<PhysicsComponent> &component) {
  if (std::find(objects.begin(), objects.end(), component) != objects.end())
    objects.erase(std::remove(objects.begin(), objects.end(), component));
}

int PhysicsEngine::getTileCollider(const glm::vec2 &position) {
  int x = static_cast<int>((position.x + 0.5f * tileSize) / tileSize);
  int y = static_cast<int>((position.y + 0.5f * tileSize) / tileSize);
  if (y >= 0 && y < collisionMap.size()) {
    if (x >= 0 && x < collisionMap[y].size())
      return collisionMap[y][x];
  }
  return 0;
}

void PhysicsEngine::getPossibleCollisions() {
  objCollisions.clear();
  for (size_t i = 0; i < objects.size(); ++i) {
    auto &obj = objects[i];
    for (size_t j = i + 1; j < objects.size(); ++j) {
      auto &colliderObj = objects[j];
      if (obj.get() == colliderObj.get() || obj->ID == colliderObj->ID)
        continue;
      float colliderRight = colliderObj->rigidBody.position.x +
                            colliderObj->collider.dimensions.x;
      float colliderBottom = colliderObj->rigidBody.position.y +
                             colliderObj->collider.dimensions.y;
      float objectRight =
          obj->rigidBody.position.x + obj->collider.dimensions.x;
      float objectBottom =
          obj->rigidBody.position.y + obj->collider.dimensions.y;
      if ((obj->rigidBody.position.x <= colliderRight &&
           objectRight >= colliderObj->rigidBody.position.x) ||
          (obj->rigidBody.position.y <= colliderBottom &&
           objectBottom >= colliderObj->rigidBody.position.y))
        objCollisions.push_back(CollisionPair(obj.get(), colliderObj.get()));
    }
  }
}

void PhysicsEngine::resolvePossibleCollisions() {
  for (auto &objs : objCollisions) {
    // Solve for two circles, we'll need to expand upon this for different
    // colliders...
    float sumOfRadius =
        objs.first->collider.dimensions.x + objs.second->collider.dimensions.x;
    glm::vec2 objFirstCenter =
        objs.first->rigidBody.position + objs.first->collider.offset;
    glm::vec2 objSecondCenter =
        objs.second->rigidBody.position + objs.second->collider.offset;
    glm::vec2 distance = objFirstCenter - objSecondCenter;
    if (glm::length(distance) < sumOfRadius) {
      // We got impact!
      glm::vec2 normal = distance / glm::length(distance);
      // That impact is a bullet hitting a gameactor!
      if ((objs.first->isBullet || objs.second->isBullet) &&
          !(objs.first->isBullet && objs.second->isBullet)) {
        eventManager->notify(std::make_shared<BulletCollideEvent>(
            (objs.first->isBullet) ? objs.first->ID : objs.second->ID,
            (!objs.first->isBullet) ? objs.first->ID : objs.second->ID,
            std::make_shared<PhysicsComponent>(
                (objs.first->isBullet) ? objs.first : objs.second),
            normal));
      }
      // Apply impulse force
      float penetrationDepth = sumOfRadius - glm::length(distance);
      glm::vec2 correctionVector =
          normal * (penetrationDepth / ((1 / objs.first->rigidBody.mass) +
                                        (1 / objs.second->rigidBody.mass)));
      glm::vec2 vrel =
          objs.first->rigidBody.velocity - objs.second->rigidBody.velocity;

      // smallest elasticity of the two colliders
      float e = std::min(objs.first->rigidBody.elasticity,
                         objs.second->rigidBody.elasticity);
      float impulseMag = (-(1 + e) * glm::dot(vrel, normal)) /
                         ((1 / objs.first->rigidBody.mass) +
                          (1 / objs.second->rigidBody.mass));

      objs.first->rigidBody.position +=
          (correctionVector / objs.first->rigidBody.mass);
      objs.second->rigidBody.position -=
          (correctionVector / objs.second->rigidBody.mass);
      objs.first->rigidBody.velocity +=
          impulseMag * normal / objs.first->rigidBody.mass;
      objs.second->rigidBody.velocity -=
          impulseMag * normal / objs.second->rigidBody.mass;
    }
  }
}

void PhysicsEngine::resolveWorldCollision(
    const std::shared_ptr<PhysicsComponent> &obj) {
  switch (obj->collider.shape) {
  case PhysicsComponent::Collider::Shape::Circle:
    float radius = obj->collider.dimensions.x;
    glm::vec2 position = obj->rigidBody.position + obj->collider.offset;
    int topTile = getTileCollider(position - glm::vec2(0, radius));
    int bottomTile = getTileCollider(position + glm::vec2(0, radius));
    int leftTile = getTileCollider(position - glm::vec2(radius, 0));
    int rightTile = getTileCollider(position + glm::vec2(radius, 0));
    if (obj->isBullet) {
      if (topTile || bottomTile || leftTile || rightTile) {
        eventManager->notify(std::make_shared<BulletDiedEvent>(obj));
        return;
      }
    }
    int tileY = static_cast<int>((position.y) / tileSize);
    int tileX = static_cast<int>((position.x) / tileSize);
    if (topTile) {
      // obj->rigidBody.velocity.y = -obj->rigidBody.velocity.y;
      obj->rigidBody.position.y =
          (tileY + 1) * tileSize + obj->collider.offset.y;
    }
    if (bottomTile) {
      // obj->rigidBody.velocity.y = -obj->rigidBody.velocity.y;
      obj->rigidBody.position.y = (tileY)*tileSize - obj->collider.offset.y;
    }
    if (leftTile) {
      // obj->rigidBody.velocity.x = -obj->rigidBody.velocity.x;
      obj->rigidBody.position.x =
          (tileX + 1) * tileSize + obj->collider.offset.x;
    }
    if (rightTile) {
      // obj->rigidBody.velocity.x = -obj->rigidBody.velocity.x;
      obj->rigidBody.position.x = (tileX)*tileSize - obj->collider.offset.x;
    }
  }
}

void PhysicsEngine::update(double deltaTime) {
  for (auto &obj : objects) {
    if (!obj)
      continue;
    glm::vec2 frictionForce = obj->rigidBody.velocity * -0.1f;
    if (std::abs(obj->rigidBody.acceleration.x) ==
        std::abs(obj->rigidBody.acceleration.y)) {
      obj->rigidBody.acceleration.x *= 0.75f;
      obj->rigidBody.acceleration.y *= 0.75f;
    }
    if (!obj->isBullet)
      obj->rigidBody.velocity += (frictionForce);
    obj->rigidBody.velocity += obj->rigidBody.acceleration;

    float maxSpeed = 500.f;
    float curSpeed = glm::length(obj->rigidBody.velocity);
    if (curSpeed > maxSpeed) {
      // Move at maxspeed
      obj->rigidBody.velocity =
          glm::normalize(obj->rigidBody.velocity) * maxSpeed;
    }
    obj->rigidBody.acceleration = glm::vec2(0.f);
    if (obj->collider.dimensions != glm::vec2(0.f)) {
      // check map collisions
      resolveWorldCollision(obj);
    }
    obj->rigidBody.position +=
        obj->rigidBody.velocity * static_cast<float>(deltaTime);
    // Make sure we keep our Z at 0.f, a better choice would be to remove the
    // need for vec3 all together!
    // TODO: REMOVE VEC3 NO NEED FOR Z COODINATE!
    // obj->rigidBody.position.z = 0.f;
  }
  getPossibleCollisions();
  if (!objCollisions.empty())
    resolvePossibleCollisions();
}
