/*
 * paused.cpp
 *
 *  Created on: 16/06/2013
 *      Author: timothyw
 */

#include "victim_id/states/lostHotSpot.hpp"

#include "victim_id/util.hpp"
#include "victim_id/states/investigate.hpp"

#include <limits>

#define LOG_START   "VIDSCentreBlob :: "

using std::string;

namespace casualty {

VIDSLostHotSpot::VIDSLostHotSpot() :
    VIDState(VID_STATE_LOST_HOT_STOP)
{}

VIDSReturnDataPtr VIDSLostHotSpot::doExecute(VIDDataPtr data) {
    //LOG("%s execute()\n", LOG_START);
    VIDSReturnDataPtr retData = new VIDSReturnData();

    // Default
    retData->nextState = VID_STATE_LOST_HOT_STOP;

    if (!continued) {
        // If changed to here (from Investigate) data has the blob to centre on
        pointSlam = data->lostHSPointSlam;
        start = ros::Time::now();
    } else if (data->blobs.size() > 0) {
        retData->nextState = VID_STATE_INVESTIGATE;
    }

    // Have we timed out?
    ros::Duration timeDiff = ros::Time::now() - start;
    if (fabs(timeDiff.toSec()) > data->config->dtcLostHotSpotTimeout) {
        LOG("%s \t\t\t lost hs timeout\n", LOG_START);
        retData->nextState = VID_STATE_INVESTIGATE;
    }

    // Update the blobPos & set skill
    if (retData->nextState == VID_STATE_LOST_HOT_STOP) {
        VIDSkillLookTrackPtr skill;
        if (continued) {
            skill = lastTrack;
        } else {
            skill = new VIDSkillLookTrack();
        }

        skill->lookTrackClient = data->lookTrackClient;
        skill->point = pointSlam;
        skill->frame = data->frameSlam;
        retData->skills.push_back(skill);
        lastTrack = skill;
    } else {
        lastTrack = NULL;
    }

    return retData;
}

} // namespace casualty

