/*
 * explore.cpp
 *
 *  Created on: 17/06/2013
 *      Author: timothyw
 */

#include "victim_id/skills/skillDriveTo.hpp"

#include <std_msgs/String.h>
#include <crosbot_explore/FollowPath.h>
#include <crosbot_explore/GetPath.h>
#include <crosbot_explore/SetMode.h>
#include <victim_id/util.hpp>

namespace casualty {

void VIDSkillDriveTo::start() {
    // TODO Make sure the point is not on the other side of the wall!!
    // Get the path given the points
    crosbot_explore::GetPathRequest requestPath;
    crosbot_explore::GetPathResponse responsePath;
    requestPath.start = robotSlam.toROS();
    requestPath.end = Pose(pointSlam, Quaternion()).toROS();
    getPathClient->call(requestPath, responsePath);

    // Set the path to follow
    crosbot_explore::FollowPathRequest requestFollow;
    crosbot_explore::FollowPathResponse responseFollow;
    requestFollow.path = responsePath.path;
    folloPathClient->call(requestFollow, responseFollow);

    // Go explorer
    crosbot_explore::SetModeRequest requestResume;
    crosbot_explore::SetModeResponse responseResume;
    requestResume.mode = EXPLORER_MODE_RESUME;
    explorerClient->call(requestResume, responseResume);

    rerun = true;
}

void VIDSkillDriveTo::stop() {
    // Publish stop command
    crosbot_explore::SetModeRequest request;
    crosbot_explore::SetModeResponse response;
    request.mode = EXPLORER_MODE_PAUSED;
    explorerClient->call(request, response);
}

bool VIDSkillDriveTo::equals(VicIdSkill& skOther) {
    VIDSkillDriveTo* other = dynamic_cast<VIDSkillDriveTo*> (&skOther);

    if (other != NULL) {
        bool retVal = true;
        retVal = retVal && other->pointSlam == pointSlam;
        return retVal;
    }

    return false;
}

} // namespace casualty
