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

#include "victim_id/states/initial.hpp"

#include "victim_id/states/explore.hpp"
#include "victim_id/skills/skillExplore.hpp"

#include <cmath>

#define LOG_START   "VIDSIntial :: "

namespace casualty {

VIDSIntial::VIDSIntial() :
    VIDState(VID_STATE_INTIAL)
{}

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

    if (!continued) {
        // We have just started
        startTime = ros::Time::now();
        retData->nextState = VID_STATE_INTIAL;
    } else {
        // Should we continue?
        ros::Duration diff = ros::Time::now() - startTime;
        if(fabs(diff.toSec()) < data->config->dtcInitialDelayedTime) {
            LOG("%s \t In initial state: diff: %.2lf, wait: %.3lf\n", LOG_START, diff.toSec(), data->config->dtcInitialDelayedTime);
            retData->nextState = VID_STATE_INTIAL;
        } else {
            retData->nextState = VID_STATE_EXPLORE;
        }
    }

    // Set skill
    if (retData->nextState == VID_STATE_INTIAL) {
        // TODO Stow arm - do in explore skill?
        // TODO Scan the head - do in explore skill?
        VIDSkillExplorePtr skExplore = new VIDSkillExplore();
        skExplore->explorerClient = data->explorerClient;
        skExplore->stowScanClient = data->stowScanClient;
        skExplore->mode = data->config->explorerBehaviour;
        retData->skills.push_back(skExplore);
    }

    return retData;
}

} // namespace casualty

