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

#include "victim_id/states/centreBlob.hpp"

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

#include <limits>

#define LOG_START   "VIDSCentreBlob :: "

using std::string;

namespace casualty {

VIDSCentreBlob::VIDSCentreBlob() :
    VIDState(VID_STATE_CENTRE_BLOB)
{}

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

    // Default
    retData->nextState = VID_STATE_CENTRE_BLOB;

    Point3D pos;
    string frame;
    bool centered = false;
    if (!continued) {
        // If changed to here (from Investigate) data has the blob to centre on
        pos = data->processSBRobotPos;
        frame = data->frameRobot;
        centered = false;
        start = ros::Time::now();
    } else if (data->blobs.size() == 0) {
        retData->nextState = VID_STATE_LOST_HOT_STOP;
        Point3D pointSlam = VIDUtil::transformPoint(data->tfListener, blobFrame, data->frameSlam, data->timeNow, blobPos);
        data->lostHSPointSlam = pointSlam;
    } else {
        // Otherwise find the closest blob to the one we were looking at
        double minDist = std::numeric_limits<double>::max();
        BlobPtr closestBlob;
        Point3D closestRobotPos;
        for(int i = 0; i != data->blobs.size(); ++i) {
            BlobPtr blob = data->blobs[i];
            Point3D robotPos = VIDUtil::transformPoint(data->tfListener, data->frameBlob, data->frameRobot, data->timestampThermal, blob->p);
            double dist = pos.distanceTo(robotPos);
            if (dist < minDist) {
                minDist = dist;
                closestBlob = blob;
                closestRobotPos = robotPos;
            }
        }

        // Set data values
        pos = closestRobotPos;
        frame = data->frameRobot;
        centered = blobIsCentered(data, closestBlob);
    }

    // If centered - then we are done - go back to investigate
    if (centered) {
        retData->nextState = VID_STATE_INVESTIGATE;
    }

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

    // Update the blobPos & set skill
    blobPos = pos;
    blobFrame = frame;
    if (retData->nextState == VID_STATE_CENTRE_BLOB) {
        //VIDSkillLookTrackPtr skill;
        VIDSkillLookAtPtr skill;
        if (continued) {
            skill = lastLookAt;
        } else {
            skill = new VIDSkillLookAt();
        }

        skill->lookAtClient = data->lookAtClient;
        skill->point = pos;
        skill->frame = frame;
        retData->skills.push_back(skill);
        //lastTrack = skill;
        lastLookAt = skill;
    } else {
        //lastTrack = NULL;
        lastLookAt = NULL;
    }

    return retData;
}

} // namespace casualty

