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

#include "victim_id/skills/skillExplore.hpp"

#include <crosbot_explore/SetMode.h>
#include <emu_arm/Scan.h>
#include <victim_id/util.hpp>

#define STOW_SCAN_MODE_STOP         0
#define STOW_SCAN_MODE_START        1
#define LOG_START                   "VIDSkillExplore ::"

namespace casualty {

void VIDSkillExplore::start() {
    // Stow the arm & scan the head
    //emu_arm::ScanRequest scanRequest;
    //emu_arm::ScanResponse scanResponse;
    //scanRequest.mode = STOW_SCAN_MODE_START;
    //LOG("%s scan mode start - %d\n", LOG_START, scanRequest.mode);
    //stowScanClient->call(scanRequest, scanResponse);

    // Set wall follow mode
    crosbot_explore::SetModeRequest requestWall;
    crosbot_explore::SetModeResponse responseWall;
    if (mode == EXPLORER_MODE_LEFT_WALL_STR) {
        requestWall.mode = EXPLORER_MODE_LEFT_WALL;
    } else {
        requestWall.mode = EXPLORER_MODE_RIGHT_WALL;
    }
    explorerClient->call(requestWall, responseWall);

    // Publish go command
    crosbot_explore::SetModeRequest requestResume;
    crosbot_explore::SetModeResponse responseResume;
    requestResume.mode = EXPLORER_MODE_RESUME;
    explorerClient->call(requestResume, responseResume);
}

void VIDSkillExplore::stop() {
    // Stop moving head/arm
    //emu_arm::ScanRequest scanRequest;
    //emu_arm::ScanResponse scanResponse;
    //scanRequest.mode = STOW_SCAN_MODE_STOP;
    //stowScanClient->call(scanRequest, scanResponse);

    // Publish stop command
    crosbot_explore::SetModeRequest request;
    crosbot_explore::SetModeResponse response;
    request.mode = EXPLORER_MODE_PAUSED;
    explorerClient->call(request, response);
}

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

    bool retVal = true;
    if (other != NULL) {
        retVal = retVal & other->mode == mode;
    } else {
        retVal = false;
    }

    return retVal;
}


} // namespace casualty
