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

#include "victim_id/states/victim.hpp"

#include "victim_id/skills/skillVictimCollect.hpp"
#include <crosbot_map/GetSnap.h>

#define LOG_START   "VIDSVictim :: "

namespace casualty {

VIDSVictim::VIDSVictim() :
    VIDState(VID_STATE_VICTIM),
    nextId(1)
{}

void VIDSVictim::init(ros::NodeHandle& nh, VIDConfigPtr config) {
    // ROS Subscribers
    colourSub = nh.subscribe(config->subColourImgName, 1, &VIDSVictim::callbackColourImage, this);
    distSub = nh.subscribe(config->subDistImgName, 1, &VIDSVictim::callbackDistanceImage, this);
}

void VIDSVictim::callbackColourImage(const sensor_msgs::ImageConstPtr& latestImage) {
    //LOG("%s Got colour image\n", LOG_START);
    colourImage = latestImage;
}

void VIDSVictim::callbackDistanceImage(const sensor_msgs::ImageConstPtr& latestImage) {
    //LOG("%s Got dist image\n", LOG_START);
    distImage = latestImage;
}

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

    retData->nextState = VID_STATE_VICTIM;

    // Look for closest QR Code with allowable range
    ImagePtr qrCodeImg = NULL;
    double minDist = data->config->dtcQrCodeRange;
    for (int i = 0; i != data->qrCodes.size(); ++i) {
        SnapPtr qrCode = data->qrCodes[i];
        Pose robotPose = qrCode->robot;            // Slam Position when captured
        Pose holePose = qrCode->pose;              // Robot relative
        Pose globalHolePose = Pose(robotPose.toTF() * holePose.toTF());

        double dist = data->robotSlamPose.position.distanceTo(globalHolePose.position);
        if(dist < minDist) {
            minDist = dist;
            qrCodeImg = NULL;

            // Load the qrcode
            crosbot_map::GetSnapRequest request;
            crosbot_map::GetSnapResponse response;
            request.id.data = qrCode->id;
            request.type.data = Snap::LANDMARK;
            data->snapGetClient->call(request, response);

            // Save the image
            if (response.snap.images.size() > 0) {
                qrCodeImg = new Image(response.snap.images[0], crosbot::Image::RGBA16);
            }
        }
    }

    // Load victim skill with data
    VIDSkillVictimCollectPtr skVitcim = new VIDSkillVictimCollect();
    skVitcim->id = nextId;
    skVitcim->snapsPub = data->snapPub;
    skVitcim->robotPoseIcp = data->robotIcpPose;
    skVitcim->victimPosRobot = data->victimPosRobot;
    skVitcim->blobImg = data->imgEncodedThermal;
    skVitcim->colourImg = new Image(colourImage);
    skVitcim->distanceImg = new Image(distImage);
    skVitcim->qrImg = qrCodeImg;
    LOG("%s \t set skVictim fields\n", LOG_START);

    retData->skills.push_back(skVitcim);
    nextId++;

    return retData;
}

} // namespace casualty

