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

#include "victim_id/states/cameraInspect.hpp"

#include "victim_id/states/investigate.hpp"
#include "victim_id/skills/skillLookAt.hpp"

#include <limits>

#define LOG_START   "VIDSCameraInspect :: "

namespace casualty {

VIDSCameraInspect::VIDSCameraInspect() :
    VIDState(VID_STATE_CAMERA_INSPECT)
{}

void VIDSCameraInspect::callbackLookAt(const std_msgs::String& response) {
    // LookAt service call returns once complete.
}

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

    // Default
    retData->nextState = VID_STATE_CAMERA_INSPECT;

    bool inspectDone = false;
    if (!continued) {
        // If changed to here (from Investigate) data has the blob to centre on
        inspectPos = data->camInspectPoint;
        inspectVector = data->camInspectVector;
        inspectFrame = data->camInspectFrame;
        start = ros::Time::now();
        camArrived = false;
    } else {
        // Look at service call returns once complete, thus if we continue, we are done
        //done = camArrived;
        inspectDone = true;
    }


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

    // Set investigate if done
    if (inspectDone) {
        retData->nextState = VID_STATE_INVESTIGATE;
    }

    // Set skill
    if (retData->nextState == VID_STATE_CAMERA_INSPECT) {
        VIDSkillLookAtPtr skill = new VIDSkillLookAt();
        skill->lookAtClient = data->lookAtClient;
        skill->point = inspectPos;
        skill->frame = inspectFrame;
        retData->skills.push_back(skill);
    }

    return retData;
}

} // namespace casualty

