#include "playercharacternode.h"

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

#include <glm/ext.hpp>

#include "scene/physicsdebugnode.h"

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

#define DEFAULT_ROTATION_SPEED 0.001f
#define TRIGGER_VALUE 15

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 RUNNING_MULTIPLIER = 2.f;
const float TORSO_RADIUS = 0.30f;
const float TORSO_HEIGHT = 0.8f;
const float LEGS_HEIGHT = 1.f;
const float EYES_OFFSET = 0.3f;
const float JUMP_VELOCITY = 5.f;


PlayerCharacterNode::PlayerCharacterNode(bool noClip) :
    m_jumping(false),
    m_noclipMode(noClip),
    m_inputActions({NO_ACTION, NO_ACTION, 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(TORSO_RADIUS, TORSO_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 run, 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[RUN] = run;
    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();
    float deltaTime = getEngine().getDeltaTime();

    float walk = 0;
    float strafe = 0;
    bool jump = false;
    bool run = false;

    if(getEngine().isMouseGrabbed())
    {
        // get events
        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[RUN])
                run = true;
            else if(action.action == m_inputActions[TOGGLE_NOCLIP])
                toggleNoClip();
        }
        glm::vec2 rotatecam = glm::vec2(0.f,0.f);
        std::vector<int> controllers = input->getControllersConnected();
        for (auto controller : controllers){
            float strafe_joy = input->getAxisPosition(controller, input::LEFT_JOYSTICK_HORIZONTAL);
            if (abs(strafe_joy) > TRIGGER_VALUE)
                strafe = strafe_joy;
            float forward = input->getAxisPosition(controller, input::LEFT_JOYSTICK_VERTICAL);
            if (abs(forward) > TRIGGER_VALUE)
                walk = -forward;
            float pitch_joy = input->getAxisPosition(controller,input::RIGHT_JOYSTICK_HORIZONTAL);
            if (abs(pitch_joy) > TRIGGER_VALUE)
                rotatecam.x = pitch_joy;
            float yaw_joy = input->getAxisPosition(controller,input::RIGHT_JOYSTICK_VERTICAL);
            if (abs(yaw_joy) > TRIGGER_VALUE)
                rotatecam.y = yaw_joy;
        }
        m_fpsCamera.rotate(rotatecam.x,rotatecam.y);
        // 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()+TORSO_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);
    glm::vec3 moveDir;
    if(walk != 0 || strafe != 0)
    {
        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;
            if(run) hPos *= RUNNING_MULTIPLIER;
            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 dir(moveDir.x/2.f,0.f,moveDir.z/2.f);
        btVector3 start_front(pos+dir);
        start_front.setY(pos.y() - TORSO_HEIGHT/2.f);
        btVector3 end_front(start_front);
        end_front.setY(end_front.y() - (LEGS_HEIGHT*2.f));
        btCollisionWorld::ClosestRayResultCallback RayCallback_front(start_front, end_front);

        btVector3 start_back(pos);
        start_back.setY(pos.y() - TORSO_HEIGHT/2.f);
        btVector3 end_back(start_back);
        end_back.setY(end_back.y() - (LEGS_HEIGHT));
        btCollisionWorld::ClosestRayResultCallback RayCallback_back(start_back, end_back);

        getEngine().getScene()->getPhysics()->rayTest(start_front, end_front, RayCallback_front);
        getEngine().getScene()->getPhysics()->rayTest(start_back, end_back, RayCallback_back);

        PhysicsDebugNode* pdnode= getEngine().getPhysicsDebug();
        if (pdnode != nullptr){
            pdnode->drawLine(start_front,end_front,btVector3(1,0,1));
            pdnode->drawLine(start_back,end_back,btVector3(1,0,1));
        }
        float controlRatio = 0.2f; // 1 = total control, 0 = no control, can be seen as a slipperiness factor

        if(RayCallback_back.hasHit()){
            onGround = true;

            //btVector3 normal_front = RayCallback_front.m_hitNormalWorld;
            btVector3 normal_back = RayCallback_back.m_hitNormalWorld;

            float slope = normal_back.dot(btVector3(0,1,0));
            controlRatio = slope > 0.4f ? 0.2f * slope : 0.f;

            if (RayCallback_front.hasHit())
            {
                float displacement = RayCallback_front.m_hitPointWorld.y() - (end_front.y() + start_front.y())/2.f;
                if(abs(displacement) > 0.1f)
                    pos.setY(pos.y() + deltaTime*0.01f*(displacement > 0 ? 1 : -1));
                else
                    pos.setY(pos.y() + displacement/4);

                btTransform transform = btTransform::getIdentity();
                transform.setOrigin(pos);
                m_rigidBody->setWorldTransform(transform);
            }

            if(jump)
                m_jumping = true;
            else
                m_jumping = false;
        }

       /* 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;

            if(onGround)
            {
                float displacement = RayCallback.m_hitPointWorld.y() - (end.y() + start.y())/2.f;
                //if (m_last_displacement > displacement){
                    if(abs(displacement) > 0.1f)
                        pos.setY(pos.y() + deltaTime*0.01f*(displacement > 0 ? 1 : -1));
                    else
                        pos.setY(pos.y() + displacement/4);
                //}
                //m_last_displacement = displacement;
                btTransform transform = btTransform::getIdentity();
                transform.setOrigin(pos);
                m_rigidBody->setWorldTransform(transform);
            }
            if(jump)
                m_jumping = true;
            else
                m_jumping = false;
        }
*/
        btVector3 newVelocity = velocity*(1.f-controlRatio) + targetVelocity*controlRatio;
        if(onGround)
        {
            if(m_jumping)
                newVelocity.setY(JUMP_VELOCITY);
            else
                newVelocity.setY(0);
        }
        m_rigidBody->setLinearVelocity(newVelocity);
    }
    m_playerLightNode->update();
    glm::vec3 p = this->getEyePosition();
    sf::Listener::setPosition(p.x,p.y,p.z);
    glm::vec3 d = getDirection();
    sf::Listener::setDirection(d.x,d.y,d.z);
}

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);
}