#include "playercharacternode.h"

#include <btBulletCollisionCommon.h>
#include <btBulletDynamicsCommon.h>

#include <glm/ext.hpp>

#include "scenetree.h"

#define DEFAULT_ROTATION_SPEED 0.01f

void FirstPersonCamera::computeView()
{
    m_view = glm::lookAt(m_eye, m_eye + m_direction, m_upVector);
}

FirstPersonCamera::FirstPersonCamera(float myFov, float myNear, float myFar) :
    BasicCamera(myFov, myNear, myFar), m_direction(0, 0, 1), m_upVector(0, 1, 0)
{
    computeView();
}

void FirstPersonCamera::move(const glm::vec3 &translation)
{
    m_eye += translation;
    computeView();
}

void FirstPersonCamera::rotate(float dx, float dy)
{
    m_direction = glm::rotate(m_direction, -dx*DEFAULT_ROTATION_SPEED, m_upVector);
    m_direction = glm::rotate(m_direction, -dy*DEFAULT_ROTATION_SPEED, glm::cross(m_direction, m_upVector));
    m_direction = glm::normalize(m_direction);
    computeView();
}

void FirstPersonCamera::moveTo(const glm::vec3 &targetPos)
{
    m_eye = targetPos;
    computeView();
}

void FirstPersonCamera::lookAt(const glm::vec3 &targetPos)
{
    m_view = glm::lookAt(m_eye, targetPos, m_upVector);
}

void FirstPersonCamera::setUpVector(const glm::vec3 &up)
{
    m_upVector = up;
    computeView();
}

const float WALK_SPEED = 0.5f;
const float PLAYER_RADIUS = 0.30f;
const float PLAYER_HEIGHT = 1.75f;
const float EYES_OFFSET = 0.775f;

PlayerCharacterNode::PlayerCharacterNode(bool noClip) :
    m_noclipMode(noClip),
    m_inputActions({NO_ACTION, NO_ACTION, NO_ACTION, NO_ACTION, NO_ACTION})
{

    m_motionState = new btDefaultMotionState();

    // Create the shape
    btCollisionShape *shape = new btCapsuleShape(PLAYER_RADIUS, PLAYER_HEIGHT);

    // Add mass
    btVector3 localInertia;
    shape->calculateLocalInertia(1.0, localInertia);

    // Create the rigid body object
    m_rigidBody = new btRigidBody(1.0, m_motionState, shape, localInertia);

    // capsule always pointing up
    m_rigidBody->setSleepingThresholds(0.0, 0.0);
    m_rigidBody->setAngularFactor(0.0);
}

void PlayerCharacterNode::setInputs(int forward, int backward, int strafeLeft, int strafeRight, int toggleNoClip)
{
    m_inputActions[FORWARD] = forward;
    m_inputActions[BACKWARD] = backward;
    m_inputActions[STRAFE_LEFT] = strafeLeft;
    m_inputActions[STRAFE_RIGHT] = strafeRight;
    m_inputActions[TOGGLE_NOCLIP] = toggleNoClip;
}

void PlayerCharacterNode::setPosition(float x, float y, float z)
{
    btTransform transform;
    btVector3 pos(x, y, z);
    transform.setOrigin(pos);
    m_rigidBody->setWorldTransform(transform);
    m_noclip_pos = pos;
    m_rigidBody->setLinearVelocity(btVector3(0,0,0));
}

void PlayerCharacterNode::update()
{
    Input *input = getEngine().getInput();
    // get events
    int walk = 0;
    int strafe = 0;
    for(int action : input->getActions())
    {
        if(action == m_inputActions[FORWARD])
            ++walk;
        else if(action == m_inputActions[BACKWARD])
            --walk;
        else if(action == m_inputActions[STRAFE_LEFT])
            --strafe;
        else if(action == m_inputActions[STRAFE_RIGHT])
            ++strafe;
        else if(action == m_inputActions[TOGGLE_NOCLIP])
            toggleNoClip();
    }

    // update camera rotation
    sf::Vector2i diff = input->getDeltaPosition();
    m_fpsCamera.rotate(diff.x, diff.y);

    // update camera position
    btVector3 pos = m_rigidBody->getCenterOfMassPosition();
    m_fpsCamera.moveTo(glm::vec3(pos.x(), pos.y()+EYES_OFFSET, pos.z()));

    // update body movement
    const glm::vec3 &glmDir = m_fpsCamera.getDirection();
    if(walk != 0 || strafe != 0)
    {
        glm::vec3 moveDir = glm::normalize(glmDir*walk + glm::cross(glmDir, glm::vec3(0, 1, 0))*strafe);
        if (m_noclipMode)
        {
            btVector3 dir(moveDir.x, moveDir.y, moveDir.z);
            m_noclip_pos += dir*WALK_SPEED*2.f;
        }
        else
        {
            //TODO: if on ground + space pressed -> jump
            glm::vec2 hPos = glm::normalize(glm::vec2(moveDir.x, moveDir.y));
            m_rigidBody->setLinearVelocity((m_rigidBody->getLinearVelocity() + btVector3(hPos.x, m_rigidBody->getLinearVelocity().getY(), hPos.y)) / 2.f); // smooth movements
        }
    }
    if(m_noclipMode)
    {
        btTransform transform;
        transform.setOrigin(m_noclip_pos);
        m_rigidBody->setWorldTransform(transform);
        m_rigidBody->setLinearVelocity(btVector3(0, 0, 0));
    }
}

void PlayerCharacterNode::toggleNoClip()
{
    m_noclipMode = !m_noclipMode;
    m_noclip_pos = m_rigidBody->getCenterOfMassPosition();
}