#include "GoalSeekToPosition.h"
#include "../SteeringBehaviours.h"
#include "../Vehicle.h"
//#include "time/CrudeTimer.h"
//#include "../navigation/Raven_PathPlanner.h"
//#include "misc/cgdi.h"

//#include "debug/DebugConsole.h"

//---------------------------- ctor -------------------------------------------
//-----------------------------------------------------------------------------
GoalSeekToPosition::GoalSeekToPosition(Vehicle* Bot, glm::vec2 target)
    :

      Goal<Vehicle>(Bot, goal_seek_to_position),
      m_Position(target),
      m_TimeToReachPos(0) {}

//---------------------------- Activate -------------------------------------
//-----------------------------------------------------------------------------
void GoalSeekToPosition::Activate() {
    m_Status = active;
    markOwnerWithGoal();

    // record the time the bot starts this goal
    m_StartTime = SDL_GetTicks();

    // This value is used to determine if the bot becomes stuck
    m_TimeToReachPos = (Uint32)m_Owner->calculateTimeToReachPosition(
        m_Position);

    // factor in a margin of error for any reactive behavior
    const Uint32 MarginOfError = 1000;

    m_TimeToReachPos += MarginOfError;

    m_Owner->getSteering()->setTarget(m_Position);
    // std::cout << "Bot " << m_Owner->getID() << " seeking to " << m_Position.x
    // << " " << m_Position.y << std::endl;
    m_Owner->getSteering()->SeekOn();
}

//------------------------------ Process --------------------------------------
//-----------------------------------------------------------------------------
int GoalSeekToPosition::Process() {
    // if status is inactive, call Activate()
    activateIfInactive();

    // test to see if the bot has become stuck
    if (isStuck()) {
        m_Status = failed;
    }

    // test to see if the bot has reached the waypoint. If so terminate the goal
    else {
        if (m_Owner->isAtPosition(m_Position)) {
            m_Status = completed;
        }
    }

    return m_Status;
}

//--------------------------- isBotStuck --------------------------------------
//
//  returns true if the bot has taken longer than expected to reach the
//  currently active waypoint
//-----------------------------------------------------------------------------
bool GoalSeekToPosition::isStuck() const {
    Uint32 TimeTaken = SDL_GetTicks() - m_StartTime;

    if (TimeTaken > m_TimeToReachPos) {
        // debug_con << "BOT " << m_Owner->ID() << " IS STUCK!!" << "";
        // std::cout << "BOT " << m_Owner->getID() << " IS STUCK!!" <<
        // std::endl;
        return true;
    }

    return false;
}

//---------------------------- Terminate --------------------------------------
//-----------------------------------------------------------------------------
void GoalSeekToPosition::Terminate() {
    m_Owner->getSteering()->SeekOff();
    m_Owner->getSteering()->ArriveOff();

    m_Status = completed;
}