#include "GoalMoveToPosition.h"
#include "../GameWorld.h"
#include "../Vehicle.h"
//#include "../navigation/Raven_PathPlanner.h"
#include "../Messaging/ArcanistMessaging.h"
#include "../Messaging/Telegram.h"
//#include "misc/cgdi.h"

#include "GoalSeekToPosition.h"
//#include "Goal_FollowPath.h"

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

    // make sure the subgoal list is clear.
    removeAllSubgoals();

    // requests a path to the target position from the path planner. Because,
    // for demonstration purposes, the Raven path planner uses time-slicing when
    // processing the path requests the bot may have to wait a few update cycles
    // before a path is calculated. Consequently, for appearances sake, it just
    // seeks directly to the target position whilst it's awaiting notification
    // that the path planning request has succeeded/failed
    // MJ taken out because we're not using pathfinder yet
    // if (m_Owner->getPathPlanner()->RequestPathToPosition(m_Destination))
    //{

    addSubgoal(new GoalSeekToPosition(m_Owner, m_Destination));
    // std::cout << "Bot " << m_Owner->getID() << " moving to " <<
    // m_Destination.x << " " << m_Destination.y << std::endl;
    //}
}

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

    // process the subgoals
    m_Status = processSubgoals();

    // if any of the subgoals have failed then this goal re-plans
    reactivateIfFailed();

    return m_Status;
}

//---------------------------- HandleMessage ----------------------------------
//-----------------------------------------------------------------------------
bool GoalMoveToPosition::HandleMessage(const Telegram&) {
    // first, pass the message down the goal hierarchy
    // TODO: COMMENTED OUT BECAUSE WASN'T BEING USED, REIMPLEMENT IF NEEDED
    // bool Handled = forwardMessageToFrontMostSubgoal(msg);
    /*
      //if the msg was not handled, test to see if this goal can handle it
      if (Handled == false)
      {
        switch(msg.Msg)
        {
        case Msg_PathReady:

          //clear any existing goals
          removeAllSubgoals();

          addSubgoal(new Goal_FollowPath(m_Owner,
                                         m_Owner->getPathPlanner()->GetPath()));

          return true; //msg handled


        case Msg_NoPathAvailable:

          m_Status = failed;

          return true; //msg handled

        default: return false;
        }
      }

      //handled by subgoals
      */
    return true;
}