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

#include "victim_id/states/driveToBlob.hpp"

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

#include <limits>

#define LOG_START   "VIDSDriveToBlob :: "

using std::string;

namespace casualty {

VIDSDriveToBlob::VIDSDriveToBlob() :
    VIDState(VID_STATE_DRIVE_TO_BLOB)
{}

void VIDSDriveToBlob::init(ros::NodeHandle& nh, VIDConfigPtr config) {
    explorerSub = nh.subscribe(config->subExplorerName, 1, &VIDSDriveToBlob::callbackExplorer, this);
}

void VIDSDriveToBlob::callbackExplorer(const std_msgs::String& response) {
    // TODO use Matt's Interface
    if (response.data == "blocked") {
        nfRobotBlocked = true;
    } else if (response.data == "arrived") {
        nfRobotArrive = true;
    }
}

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

    // Default
    retData->nextState = VID_STATE_DRIVE_TO_BLOB;

    Point3D pos;
    string frame;
    bool stopDriving;
    if (!continued) {
        // If changed to here (from Investigate) data has the blob to centre on
        pos = data->processSBSlamPos;
        frame = data->frameSlam;
        nfRobotArrive = false;
        nfRobotBlocked = false;
        start = ros::Time::now();
    } else if (nfRobotArrive || nfRobotBlocked) {
        stopDriving = true;
    } else if (data->blobs.size() == 0) {
        retData->nextState = VID_STATE_LOST_HOT_STOP;
        Point3D pointSlam = VIDUtil::transformPoint(data->tfListener, driveToFrame, data->frameSlam, data->timeNow, driveToPos);
        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;

        // If within range
        if (closestBlob->averageDistance <= data->config->dtcDistVictimHS) {
            stopDriving = true;
        }
    }

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

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

    if (retData->nextState == VID_STATE_DRIVE_TO_BLOB) {
        // Convert frame if necessary
        if (frame != data->frameSlam) {
            pos = VIDUtil::transformPoint(data->tfListener, frame, data->frameSlam, data->timeNow, pos);
            frame = data->frameSlam;
        }
        driveToPos = pos;
        driveToFrame = frame;

        VIDSkillDriveToPtr skDrive;
        VIDSkillLookTrackPtr skLookTrack;
        if (continued) {
            skDrive = lastDriveTo;
            skLookTrack = lastLookTrack;
        } else {
            skDrive = new VIDSkillDriveTo();
            skLookTrack = new VIDSkillLookTrack();
        }

        skDrive->explorerClient = data->explorerClient;
        skDrive->folloPathClient = data->followPathClient;
        skDrive->getPathClient = data->getPathClient;
        skDrive->robotSlam = data->robotSlamPose;
        skDrive->pointSlam = driveToPos;
        skLookTrack->lookTrackClient= data->lookTrackClient;
        skLookTrack->point = driveToPos;
        skLookTrack->frame = frame;

        retData->skills.push_back(skDrive);
        retData->skills.push_back(skLookTrack);
        lastDriveTo = skDrive;
        lastLookTrack = skLookTrack;
    } else {
        lastDriveTo = NULL;
        lastLookTrack = NULL;
    }

    return retData;
}

} // namespace casualty

