#include "playercharacternode.h"

#include <btBulletCollisionCommon.h>
#include <btBulletDynamicsCommon.h>
#include <BulletDynamics/Character/btKinematicCharacterController.h>

#include <glm/ext.hpp>

#include "scenetree.h"
#include "lightnode.h"
#include <SFML/System.hpp>

#define DEFAULT_ROTATION_SPEED 0.001f

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 = 5.f;
const float PLAYER_RADIUS = 0.30f;
const float PLAYER_HEIGHT = 1.75f;
const float EYES_OFFSET = 0.775f;
const float JUMP_VELOCITY = 5.f;
const float EPSILON = 1.5f;

PlayerCharacterNode::PlayerCharacterNode(bool noClip) :
    m_noclipMode(noClip),
    m_inputActions({NO_ACTION, NO_ACTION, NO_ACTION, NO_ACTION, NO_ACTION})
{
    m_playerLight = new PointLight(glm::vec3(150, 10, 30), 10, glm::vec3(0.18f, 0.16f, 0.096f)*2.f);
    m_playerLightNode = new LightNode(m_playerLight);
    m_playerLightNode->m_parent = this;

    // 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, nullptr, shape, localInertia);

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

PlayerCharacterNode::~PlayerCharacterNode()
{
    m_playerLightNode->setSceneTree(nullptr);
    delete m_playerLightNode;
    delete m_playerLight;
}

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

void PlayerCharacterNode::setPosition(float x, float y, float z)
{
    btTransform transform = btTransform::getIdentity();
    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;
    bool jump = false;
    for(Action action : input->getActions())
    {
        if(action.action == m_inputActions[FORWARD])
            ++walk;
        else if(action.action == m_inputActions[BACKWARD])
            --walk;
        else if(action.action == m_inputActions[STRAFE_LEFT])
            --strafe;
        else if(action.action == m_inputActions[STRAFE_RIGHT])
            ++strafe;
        else if(action.action == m_inputActions[JUMP])
            jump = true;
        else if(action.action == m_inputActions[TOGGLE_NOCLIP])
            toggleNoClip();
    }

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

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

    // update body movement
    const glm::vec3 &glmDir = m_fpsCamera.getDirection();
    const btVector3 &velocity = m_rigidBody->getLinearVelocity();
    btVector3 targetVelocity(0.f, velocity.getY(), 0.f);
    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*getEngine().getDeltaTime()*0.004f;
        }
        else
        {
            glm::vec2 hPos = glm::normalize(glm::vec2(moveDir.x, moveDir.z))*WALK_SPEED;
            targetVelocity.setX(hPos.x);
            targetVelocity.setZ(hPos.y);
        }
    }
    // apply movements
    if(m_noclipMode)
    {
        btTransform transform = btTransform::getIdentity();
        transform.setOrigin(m_noclip_pos);
        m_rigidBody->setWorldTransform(transform);
        m_rigidBody->setLinearVelocity(btVector3(0, 0, 0));
    }
    else
    {
        bool onGround = false;
        btVector3 start(pos);
        start.setY(start.y() - PLAYER_HEIGHT/2.f);
        btVector3 end(pos);
        end.setY(end.y() - EPSILON);
        btCollisionWorld::ClosestRayResultCallback RayCallback(start, end);
        getEngine().getPhysics()->rayTest(start, end, RayCallback);
        float controlRatio = 0.f; // 1 = total control, 0 = no control, can be seen as a slipperiness factor
        if(RayCallback.hasHit()) // if ground is nearby
        {
            onGround = true;
            btVector3 normal = RayCallback.m_hitNormalWorld;
            float slope = normal.dot(btVector3(0, 1, 0));
            controlRatio = slope > 0.4f ? 0.2f * slope : 0.f;
        }

        btVector3 newVelocity = velocity*(1.f-controlRatio) + targetVelocity*controlRatio;
        if(jump && onGround)
            newVelocity.setY(JUMP_VELOCITY);
        m_rigidBody->setLinearVelocity(newVelocity);
    }
    m_playerLightNode->update();
}

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

void PlayerCharacterNode::setSceneTree(SceneTree* tree)
{
    m_scene = tree;
    m_playerLightNode->setSceneTree(tree);
}