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

#include "victim_id/skills/skillVictimCollect.hpp"

#define LOG_START       "VIDSkillVictimCollect ::"

#include <crosbot_map/tag.hpp>
#include <crosbot_map/SnapMsg.h>

namespace casualty {

void VIDSkillVictimCollect::start() {
    // Create Snap
    LOG("%s start()\n", LOG_START);
    SnapPtr snap = new Snap();
    snap->id = id;
    snap->type = Snap::VICTIM;
    snap->status = Snap::UNCONFIRMED;
    snap->description = "Autonomous Victim " + id;
    snap->robot = robotPoseIcp;
    snap->pose.position = victimPosRobot;

    // Add images
    if (blobImg != NULL) snap->images.push_back(blobImg);
    if (colourImg != NULL) snap->images.push_back(colourImg);
    if (distanceImg != NULL) snap->images.push_back(distanceImg);
    if (qrImg != NULL) snap->images.push_back(qrImg);

    // Publish New Snap
    LOG("%s \t publish\n", LOG_START);
    snapsPub->publish(snap->toROS());
}

void VIDSkillVictimCollect::stop() {
    // Stop does nothing
}

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

    if (other != NULL) {
        return true;
    }

    return false;
}

} // namespace casualty
