#include "StdAfx.h"
#include "CharacterPath.h"
#include "Character/Character.h"
#include "Character/Movement/CharacterMovement.h"


CCharacterPath::CCharacterPath()
    : m_movementRequestId(0)
    , m_pPathFollower(nullptr)
    , m_pFoundPath(nullptr)
{
}

CCharacterPath::~CCharacterPath()
{
    Reset();
}

void CCharacterPath::PostInit(IGameObject *pGameObject)
{
    InitAI();
}

void CCharacterPath::Reset()
{
    gEnv->pAISystem->GetMovementSystem()->UnregisterEntity(GetEntityId());
    m_pPathFollower.reset();
    SAFE_RELEASE(m_pFoundPath);
}

void CCharacterPath::InitAI()
{
    Reset();

    m_pCharacter = static_cast<CCharacter *>(GetGameObject()->QueryExtension("Character"));

    m_navigationAgentTypeId = gEnv->pAISystem->GetNavigationSystem()->GetAgentTypeID("MediumSizedCharacters");

    m_callbacks.queuePathRequestFunction = functor(*this, &CCharacterPath::RequestPathTo);
    m_callbacks.checkOnPathfinderStateFunction = functor(*this, &CCharacterPath::GetPathfinderState);
    m_callbacks.getPathFollowerFunction = functor(*this, &CCharacterPath::GetPathFollower);
    m_callbacks.getPathFunction = functor(*this, &CCharacterPath::GetINavPath);

    gEnv->pAISystem->GetMovementSystem()->RegisterEntity(GetEntityId(), m_callbacks, *this);

    if (m_pPathFollower == nullptr)
    {
        PathFollowerParams params;
        params.maxAccel = m_pCharacter->GetCVars().m_moveSpeed;
        params.maxSpeed = m_pCharacter->GetCVars().m_moveSpeed;
        params.minSpeed = 0.0f;
        params.normalSpeed = params.maxSpeed;

        params.use2D = false;

        m_pPathFollower = gEnv->pAISystem->CreateAndReturnNewDefaultPathFollower(params, m_pathObstacles);
    }

    m_movementAbility.b3DMove = true;
}

void CCharacterPath::RequestMoveTo(const Vec3 &position)
{
    CRY_ASSERT_MESSAGE(m_movementRequestId.id == 0, "RequestMoveTo can not be called while another request is being handled!");

    MovementRequest movementRequest;
    movementRequest.entityID = GetEntityId();
    movementRequest.destination = position;
    movementRequest.callback = functor(*this, &CCharacterPath::MovementRequestCallback);
    movementRequest.style.SetSpeed(MovementStyle::Walk);

    movementRequest.type = MovementRequest::Type::MoveTo;

    m_state = Movement::StillFinding;

    m_movementRequestId = gEnv->pAISystem->GetMovementSystem()->QueueRequest(movementRequest);
}

void CCharacterPath::RequestPathTo(MNMPathRequest &request)
{
    m_state = Movement::StillFinding;

    request.resultCallback = functor(*this, &CCharacterPath::OnMNMPathResult);
    request.agentTypeID = m_navigationAgentTypeId;

    m_pathFinderRequestId = gEnv->pAISystem->GetMNMPathfinder()->RequestPathTo(this, request);
}

void CCharacterPath::CancelCurrentRequest()
{
    CRY_ASSERT(m_movementRequestId.id != 0);

    gEnv->pAISystem->GetMovementSystem()->CancelRequest(m_movementRequestId);
    m_movementRequestId = 0;

    if (m_pathFinderRequestId != 0)
    {
        gEnv->pAISystem->GetMNMPathfinder()->CancelPathRequest(m_pathFinderRequestId);

        m_pathFinderRequestId = 0;
    }
}

void CCharacterPath::OnMNMPathResult(const MNM::QueuedPathID& requestId, MNMPathRequestResult& result)
{
    m_pathFinderRequestId = 0;

    if (result.HasPathBeenFound())
    {
        m_state = Movement::FoundPath;

        SAFE_DELETE(m_pFoundPath);
        m_pFoundPath = result.pPath->Clone();

        // Bump version
        m_pFoundPath->SetVersion(m_pFoundPath->GetVersion() + 1);

        m_pPathFollower->Reset();
        m_pPathFollower->AttachToPath(m_pFoundPath);
    }
    else
    {
        m_state = Movement::CouldNotFindPath;
    }
}


Vec3 CCharacterPath::GetVelocity() const
{
    return m_pCharacter->GetMovement()->GetVelocity();
}

void CCharacterPath::SetMovementOutputValue(const PathFollowResult& result)
{
    float frameTime = gEnv->pTimer->GetFrameTime();
    float moveStep = m_pCharacter->GetCVars().m_moveSpeed * frameTime;

    auto &playerMovement = *m_pCharacter->GetMovement();

    if (result.velocityOut.GetLength() > moveStep)
    {
        playerMovement.RequestMove(result.velocityOut.GetNormalized() * moveStep);
    }
    else
    {
        playerMovement.SetVelocity(result.velocityOut);
    }
}

void CCharacterPath::ClearMovementState()
{
    m_pCharacter->GetMovement()->SetVelocity(ZERO);
}