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

#include "victim_id/skills/skillLookTrack.hpp"

#include <emu_arm/Track.h>
#include <victim_id/util.hpp>

namespace casualty {

void VIDSkillLookTrack::start() {
    // Load point
    geometry_msgs::PointStamped pointStamped;
    pointStamped.point = point.toROS();
    pointStamped.header.frame_id = frame;

    // Service call
    emu_arm::TrackRequest request;
    emu_arm::TrackResponse response;
    request.point = pointStamped;
    request.mode = EMU_ARM_LOOKAT_MODE_START;
    lookTrackClient->call(request, response);

    rerun = true;
}

void VIDSkillLookTrack::stop() {
    // Service call
    geometry_msgs::PointStamped pointStamped;
    pointStamped.point = point.toROS();
    pointStamped.header.frame_id = frame;
    emu_arm::TrackRequest request;
    emu_arm::TrackResponse response;
    request.point = pointStamped;
    request.mode = EMU_ARM_LOOKAT_MODE_STOP;
    lookTrackClient->call(request, response);
}

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

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

    return false;
}

} // namespace casualty
