#include "defaultplayercontroller.h"

#include "internal.h"
#include "internal/physics.h"

#include <glm/gtx/vector_angle.hpp>
#include <BulletCollision/CollisionDispatch/btCollisionWorld.h>

namespace kek {

static bool castPlayer(glm::vec3 pos, glm::vec3 delta, const btCollisionObject **outBody, float *outFrac, glm::vec3 *outPoint, glm::vec3 *outNormal) {
	btTransform from;
	from.setIdentity();
	from.setOrigin(Physics::fromGLM(pos));

	btTransform to;
	to.setIdentity();
	to.setOrigin(Physics::fromGLM(pos + delta));

	ClosestNotSelfConvexResultCallback cb = ClosestNotSelfConvexResultCallback(from.getOrigin(), to.getOrigin(), kekData.player->physics->body);
	cb.m_collisionFilterGroup = -1;
	cb.m_collisionFilterMask = -1;
	kekData.physics->world->convexSweepTest((btConvexShape *) kekData.player->physics->body->getCollisionShape(), from, to, cb);
	if(!cb.m_hitCollisionObject) return false;

	if(outBody) *outBody = cb.m_hitCollisionObject;
	if(outFrac) *outFrac = cb.m_closestHitFraction;
	if(outPoint) *outPoint = Physics::toGLM(cb.m_hitPointWorld);
	if(outNormal) *outNormal = Physics::toGLM(cb.m_hitNormalWorld);
	return true;
}

bool DefaultPlayerController::checkPlayerGrounded() {
	glm::vec3 groundDir = glm::vec3(0,-groundDistance,0);

	glm::vec3 point;
	glm::vec3 normal;
	bool onGround = castPlayer(kekData.player->getPosition(), groundDir, nullptr, nullptr, &point, &normal);
	if(!onGround) return false;

	normal = glm::normalize(normal);

	float angle = 180.0f - glm::angle(normal, glm::normalize(groundDir)) / M_PI * 180.0f;
	if(angle > maxWalkAngle) return false;

	return true;
}

DefaultPlayerController::DefaultPlayerController() {

}

DefaultPlayerController::~DefaultPlayerController() {

}

glm::vec3 DefaultPlayerController::move(glm::vec3 movement) {
	glm::vec3 pos = kekData.player->getPosition();
	glm::vec3 totalMovement = glm::vec3(0);

	int count = 0;
	while (count < 3) {
		float frac;
		glm::vec3 normal;
		bool collision = castPlayer(pos + totalMovement, movement, nullptr, &frac, nullptr, &normal);
		if(!collision) {
			totalMovement += movement;
			break;
		}

		normal = glm::normalize(normal);

		totalMovement += movement * frac;

		float angle = 180.0f - glm::angle(normal, glm::normalize(movement)) / M_PI * 180.0f;
		if(angle < minSlideAngle) break;

		totalMovement += normal * 0.001f;

		glm::vec3 remainingProjected = glm::normalize(glm::cross(normal, glm::cross(movement, normal))) * glm::length(movement) * (1 - frac);
		movement = remainingProjected;
		count++;
	}

	kekData.player->translate(totalMovement);
	return totalMovement;
}

void DefaultPlayerController::update() {
	bool onGround = checkPlayerGrounded();
	kekData.player->onGround = onGround;

	if(!onGround) {
		velocity += gravity * kekData.lastFrameTime;
	}else{
		velocity = glm::vec3(0);
	}

	glm::vec3 direction = glm::vec3(0);

	if(Input::getKeyState(keyForward) == GLFW_PRESS) {
		direction += kekData.activeCamera->direction;
	}

	if(Input::getKeyState(keyBackward) == GLFW_PRESS) {
		direction += -kekData.activeCamera->direction;
	}

	if(Input::getKeyState(keyLeft) == GLFW_PRESS) {
		direction += -glm::normalize(glm::cross(kekData.activeCamera->direction, glm::vec3(0.0f, 1.0f, 0.0f)));
	}

	if(Input::getKeyState(keyRight) == GLFW_PRESS) {
		direction += glm::normalize(glm::cross(kekData.activeCamera->direction, glm::vec3(0.0f, 1.0f, 0.0f)));
	}

	if(Input::getKeyState(keyJump) == GLFW_PRESS && kekData.player->onGround) {
		velocity += glm::vec3(0, jumpVelocity, 0);
	}

	direction = glm::vec3(direction.x, 0, direction.z);

	glm::vec3 move = velocity;
	if(glm::length2(direction) > 0) {
		direction = glm::normalize(direction) * walkSpeed;
		move += direction;
	}

	kekData.player->controller->move(move * (kekData.lastFrameTime));
}

}