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

#include "utility/logger.h"

#include <iostream>

void PhysicsEngine::hookEventManager(const std::shared_ptr<EventManager>& eventManager)
{
	this->eventManager = eventManager;
	this->eventManager->subscribe("OnBulletFired", [this](std::shared_ptr<Event> e) {
		auto bulletEvent = std::static_pointer_cast<BulletFiredEvent>(e);
		this->addObject(bulletEvent->bullet->getPhysicsComponent());
	});
	this->eventManager->subscribe("OnBulletDied", [this](std::shared_ptr<Event> e) {
		auto bulletEvent = std::static_pointer_cast<BulletDiedEvent>(e);
		this->removeObject(bulletEvent->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.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::vec3& 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::vec3 objFirstCenter = objs.first->rigidBody.position + objs.first->collider.offset;
		glm::vec3 objSecondCenter = objs.second->rigidBody.position + objs.second->collider.offset;
		glm::vec3 distance = objFirstCenter - objSecondCenter;
		if (glm::length(distance) < sumOfRadius)
		{
			// We got impact!
			glm::vec3 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::vec3 correctionVector = normal * (penetrationDepth / ((1 / objs.first->rigidBody.mass) + (1 / objs.second->rigidBody.mass)));
			glm::vec3 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::vec3 position = obj->rigidBody.position + obj->collider.offset;
		int topTile = getTileCollider(position - glm::vec3(0, radius, 0));
		int bottomTile = getTileCollider(position + glm::vec3(0, radius, 0));
		int leftTile = getTileCollider(position - glm::vec3(radius, 0, 0));
		int rightTile = getTileCollider(position + glm::vec3(radius, 0, 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::vec3 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::vec3(0.f);
		if (obj->collider.dimensions != glm::vec3(0.f))
		{
			// check map collisions
			resolveWorldCollision(obj);
		}
		obj->rigidBody.position += obj->rigidBody.velocity * static_cast<float>(deltaTime);
	}
	getPossibleCollisions();
	if (!objCollisions.empty())
		resolvePossibleCollisions();
}