/*
 * victimId.cpp
 *
 *  Created on: 03/06/2013
 *      Author: timothyw
 */

#include "victim_id/victimId.hpp"

#include <crosbot/utils.hpp>
#include <crosbot_explore/FollowPath.h>
#include <crosbot_explore/GetPath.h>
#include <crosbot_explore/SetMode.h>
#include <crosbot_map/GetSnap.h>
#include <crosbot_map/ListSnaps.h>
#include <crosbot_map/ModifySnap.h>
#include <crosbot_map/SnapMsg.h>
#include <crosbot_map/tag.hpp>
#include <emu_arm/LookAt.h>
#include <emu_arm/Track.h>
#include <emu_arm/Scan.h>
#include <thermal_blobber/BlobsService.h>
#include <hole_detector/HoleDetectorService.h>

#include "victim_id/util.hpp"
#include "victim_id/states/cameraInspect.hpp"
#include "victim_id/states/centreBlob.hpp"
#include "victim_id/states/driveToBlob.hpp"
#include "victim_id/states/explore.hpp"
#include "victim_id/states/initial.hpp"
#include "victim_id/states/investigate.hpp"
#include "victim_id/states/lostHotSpot.hpp"
#include "victim_id/states/paused.hpp"
#include "victim_id/states/victim.hpp"
#include "victim_id/VictimIdDataMsg.h"

#include <std_msgs/String.h>

#define DEFAULT_BESIDE_MIN              70
#define DEFAULT_BESIDE_MAX              110
#define DEFAULT_BLOB_CELLING            2.5
#define DEFAULT_BLOB_DEF_HEIGHT         2.0
#define DEFAULT_BLOB_HOLE_DIST          0.5
#define DEFAULT_CAMERA_INSPECT_TIMEOUT  10.0
#define DEFAULT_CENTRE_FRACTION         0.5
#define DEFAULT_CENTRE_TIMEOUT          10.0
#define DEFAULT_DCONSHS                 3.0
#define DEFAULT_DELAYED_RADIUS          0.2
#define DEFAULT_DELAYED_MAX_ANGLE       20
#define DEFAULT_DEVIATE_ANGLE           30
#define DEFAULT_DRIVE_TO_TIMEOUT        60
#define DEFAULT_DVICHS                  1.0
#define DEFAULT_FREQUENCY               1
#define DEFAULT_INITIAL_DELAY           120
#define DEFAULT_INSPECT_POINT_THRES     0.5
#define DEFAULT_FALSE_POS_RADIUS        0.2
#define DEFAULT_LIFE_DELAYED            90
#define DEFAULT_LOST_HOT_SPOT_TIMEOUT   5
#define DEFAULT_MAXWAIT_SERVICE         2.0
#define DEFAULT_QR_CODE_RANGE           1.0
#define DEFAULT_TIMEOUT                 1.0
#define DEFAULT_TIMEOUT_DTONLY          0.5
#define DEFAULT_ROBOT_LENGTH            0.4
#define DEFAULT_ROBOT_RADIUS            0.2
#define DEFAULT_SE_BEHAVIOUR            EXPLORER_MODE_RIGHT_WALL_STR
#define DEFAULT_TIMEOUT_INVEST          30.0
#define DEFAULT_TRAVEL_DELAYED          2.0
#define DEFAULT_VICTIM_RADIUS           1.0
#define LOG_START                       "AutoVictimId :: "

#define SEC2USEC(n) (n * 1000000)

namespace casualty {

// Private function(s) to this file
bool sortSwissBlobsReverse(BlobPtr a, BlobPtr b) {
    return a->averageDistance >= b->averageDistance;
}

AutoVicPing::AutoVicPing(std::string name, AutoVictimId* autoVictimId, double frequency) :
    Thread(name),
    autoVictimid(autoVictimId),
    frequency(SEC2USEC(frequency))
{}

void AutoVicPing::run() {
    LOG("AutoVicPing :: Running\n");
    while (operating) {
        usleep(frequency);
        if (operating) {
        	autoVictimid->notifySemaphore();
        }
    }
    LOG("%s run() terminated\n", LOG_START);
}

void AutoVicPing::start() {
    operating = true;
    Thread::start();
}

void AutoVicPing::stop() {
    LOG("AutoVicPing :: stop\n");
    operating = false;
}

AutoVictimId::AutoVictimId() :
		Thread("AutoVictimId", false),
		operating(false),
		paused(true),
		onlyRunDT(false),
		nextHoleId(1)
{};

void AutoVictimId::configure(ConfigElementPtr configElem) {
	LOG("%s configure()\n", LOG_START);
	config = new VIDConfig();

	frameRobot                      = configElem->getParam("frameRobot", "/base_footprint");
	frameIcp                        = configElem->getParam("frameIcp", "/icp");
	frameSlam                       = configElem->getParam("frameSlam", "/slam_frame");
	frequency                       = configElem->getParamAsDouble("frequency", DEFAULT_FREQUENCY);
	pubSnapsName                    = configElem->getParam("pubSnaps", "out_snaps");
	pubVicDataName                  = configElem->getParam("pubVicData", "victim_data_out");
	config->subExplorerName         = configElem->getParam("subExplorer", "explorer_in");
	config->subLookAtName           = configElem->getParam("subLookAt", "look_at_in");
	config->subColourImgName        = configElem->getParam("subColourImg", "colour_img_in");
	config->subDistImgName          = configElem->getParam("subDistImg", "dist_img_in");
	serviceBlobberName              = configElem->getParam("serviceBlobs", "in_blobs");
    serviceExplorerName             = configElem->getParam("serviceExplorer", "explorer_commands");
    serviceFollowPathName           = configElem->getParam("serviceFollowPath", "explorer_follow_path");
    serviceGetPathName              = configElem->getParam("serviceGetPath", "explorer_get_path");
	serviceHolesName                = configElem->getParam("serviceHoles", "in_holes");
	serviceLookAtName               = configElem->getParam("serviceLookAt", "look_at_commands");
	serviceLookTrackName            = configElem->getParam("serviceLookTrack", "look_track_commands");
	serviceMapGetSnapsName          = configElem->getParam("serviceMapGetSnaps", "map_get_snaps");
	serviceSnapGetName              = configElem->getParam("serviceSnapGet", "snap_get");
	serviceSnapUpdateName           = configElem->getParam("serviceSnapUpdate", "snap_update");
	serviceStowScanName             = configElem->getParam("serviceStowScan", "stow_scan");

    config->dtcBesideMin            = DEG2RAD(configElem->getParamAsDouble("besideMin", DEFAULT_BESIDE_MIN));
    config->dtcBesideMax            = DEG2RAD(configElem->getParamAsDouble("besideMax", DEFAULT_BESIDE_MAX));
    config->dtcBlobCelling          = configElem->getParamAsDouble("blobCelling", DEFAULT_BLOB_CELLING);
    config->dtcBlobDefHeight        = configElem->getParamAsDouble("blobDefHeight", DEFAULT_BLOB_DEF_HEIGHT);
    config->dtcBlobHoleDist         = configElem->getParamAsDouble("blobHoleDist", DEFAULT_BLOB_HOLE_DIST);
    config->dtcCameraInspectTimeout = configElem->getParamAsDouble("cameraInspectTimeout", DEFAULT_CAMERA_INSPECT_TIMEOUT);
    config->dtcCentreFraction       = configElem->getParamAsDouble("centreFraction", DEFAULT_CENTRE_FRACTION);
    config->dtcCentreTimeout        = configElem->getParamAsDouble("centreTimeout", DEFAULT_CENTRE_TIMEOUT);
    config->dtcDistConsiderHS       = configElem->getParamAsDouble("distConsiderHS", DEFAULT_DCONSHS);
    config->dtcDistVictimHS         = configElem->getParamAsDouble("distVictimHS", DEFAULT_DVICHS);
    config->dtcDelayedMaxAngle      = DEG2RAD(configElem->getParamAsDouble("delayexMaxAngle", DEFAULT_DELAYED_MAX_ANGLE));
    config->dtcDelayedRadius        = configElem->getParamAsDouble("delayedRadius", DEFAULT_DELAYED_RADIUS);
    config->dtcDeviatePathAngle     = DEG2RAD(configElem->getParamAsInt("deviatePathAngle", DEFAULT_DEVIATE_ANGLE));
    config->dtcDriveToTimeout       = configElem->getParamAsDouble("driveToTimeout", DEFAULT_DRIVE_TO_TIMEOUT);
    config->dtcInitialDelayedTime   = configElem->getParamAsDouble("initialDelay", DEFAULT_INITIAL_DELAY);
    config->dtcFalsePositiveRadius  = configElem->getParamAsDouble("falsePositiveRadius", DEFAULT_FALSE_POS_RADIUS);
    config->dtcLifeDelayed          = configElem->getParamAsDouble("lifeDelayed", DEFAULT_LIFE_DELAYED);
    config->dtcLostHotSpotTimeout   = configElem->getParamAsDouble("lostHotSpotTimeout", DEFAULT_LOST_HOT_SPOT_TIMEOUT);
    config->dtcQrCodeRange          = configElem->getParamAsDouble("qrCodeRange", DEFAULT_QR_CODE_RANGE);
    config->dtcRerunTimeout         = SEC2USEC(configElem->getParamAsDouble("rerunTimeout", DEFAULT_TIMEOUT));
    config->dtcRerunTimeoutDTOnly   = SEC2USEC(configElem->getParamAsDouble("rerunTimeoutDtOnly", DEFAULT_TIMEOUT_DTONLY));
    config->dtcRobotRadius          = configElem->getParamAsDouble("robotRadius", DEFAULT_ROBOT_RADIUS);
    config->dtcTimeoutInvest        = configElem->getParamAsDouble("timeoutInvestigate", DEFAULT_TIMEOUT_INVEST);
    config->dtcTravelDelayed        = configElem->getParamAsDouble("travelDelayed", DEFAULT_TRAVEL_DELAYED);
    config->dtcVictimRadius         = configElem->getParamAsDouble("victimRadius", DEFAULT_VICTIM_RADIUS);

    // Explorer behaviour
    config->explorerBehaviour       = configElem->getParam("explorerBehaviour", DEFAULT_SE_BEHAVIOUR);

    // Other config parameters
    config->robotLength             = configElem->getParamAsDouble("robotLength", DEFAULT_ROBOT_LENGTH);

	LOG("%s \t frequency: %lf\n", LOG_START, frequency);
}

void AutoVictimId::init(ros::NodeHandle& nh) {
    LOG("%s init()\n", LOG_START);
	ping = new AutoVicPing("AutoVicPing", this, frequency);

    // Load ROS services
    blobberClient = nh.serviceClient<thermal_blobber::BlobsService>(serviceBlobberName, false);
    explorerClient = nh.serviceClient<crosbot_explore::SetMode>(serviceExplorerName, false);
    followPathClient = nh.serviceClient<crosbot_explore::FollowPath>(serviceFollowPathName, false);
    getPathClient = nh.serviceClient<crosbot_explore::GetPath>(serviceGetPathName, false);
    holesClient = nh.serviceClient<hole_detector::HoleDetectorService>(serviceHolesName, false);
    lookAtClient = nh.serviceClient<emu_arm::LookAt>(serviceLookAtName, false);
    lookTrackClient = nh.serviceClient<emu_arm::Track>(serviceLookTrackName, false);
    mapGetSnapsClient = nh.serviceClient<crosbot_map::ListSnaps>(serviceMapGetSnapsName, false);
    snapGetClient = nh.serviceClient<crosbot_map::GetSnap>(serviceSnapGetName, false);
    snapUpdateClient = nh.serviceClient<crosbot_map::ModifySnap>(serviceSnapUpdateName, false);
    stowScanClient = nh.serviceClient<emu_arm::Scan>(serviceStowScanName, false);

    // Load ROS publishers
    snapPub = nh.advertise<crosbot_map::SnapMsg>(pubSnapsName, 1);
    vicDataPub = nh.advertise<victim_id::VictimIdDataMsg>(pubVicDataName, 1);
    //depthProcPubColour = nh.advertise<sensor_msgs::Image>("depth_proc_colour", 1);
    //depthProcPubDepth = nh.advertise<sensor_msgs::Image>("depth_proc_depth", 1);

    // Load ROS subscriptions

    // Load TF Listener
    tfListener = new tf::TransformListener();

    // Save nh reference
    this->nh = nh;
}

void AutoVictimId::run() {
	LOG("%s run()\n", LOG_START);

	// Load states
	stateMap[VID_STATE_CAMERA_INSPECT] = new VIDSCameraInspect();
	stateMap[VID_STATE_CENTRE_BLOB] = new VIDSCentreBlob();
	stateMap[VID_STATE_DRIVE_TO_BLOB] = new VIDSDriveToBlob();
	stateMap[VID_STATE_EXPLORE] = new VIDSExplore();
	stateMap[VID_STATE_INTIAL] = new VIDSIntial();
	stateMap[VID_STATE_INVESTIGATE] = new VIDSInvestigate();
	stateMap[VID_STATE_LOST_HOT_STOP] = new VIDSLostHotSpot();
	stateMap[VID_STATE_PAUSED] = new VIDSPaused();
	stateMap[VID_STATE_VICTIM] = new VIDSVictim();

	// Init states
	stateMap[VID_STATE_CAMERA_INSPECT]->init(nh, config);
	stateMap[VID_STATE_CENTRE_BLOB]->init(nh, config);
	stateMap[VID_STATE_DRIVE_TO_BLOB]->init(nh, config);
	stateMap[VID_STATE_EXPLORE]->init(nh, config);
	stateMap[VID_STATE_INTIAL]->init(nh, config);
	stateMap[VID_STATE_INVESTIGATE]->init(nh, config);
	stateMap[VID_STATE_LOST_HOT_STOP]->init(nh, config);
	stateMap[VID_STATE_PAUSED]->init(nh, config);
    stateMap[VID_STATE_VICTIM]->init(nh, config);

	// Load initial variable values
	data = NULL;
	currentState = stateMap[VID_STATE_PAUSED];
	previousState = VID_STATE_NO_STATE;
	timeLastExplore = ros::Time::now();

    // Start ping thread
    ping->start();

	// Setup for execute
	operating = true;
	paused = true;
	onlyRunDT = false;

	// Execute
	execute();
}

void AutoVictimId::stop() {
	// Grab processing lock
    Lock processLock(dtProcessMutex);
	LOG("%s stopping\n", LOG_START);

	// Terminate
	paused = true;
	operating = false;

    // Stop ping
    ping->stop();

    // Clear data
    data = NULL;
    stateMap.clear();
    currentState = NULL;
    previousState = VID_STATE_NO_STATE;

	// Hit process to clear thread
	processSemaphore.notify();
}

void AutoVictimId::setPaused(bool paused) {
    LOG("%s setPaused() - grabbing locks\n", LOG_START);
    {
        Lock stateLock(dtStateMutex);
        Lock skillLock(dtSkillMutex);
        LOG("%s \t Locks acquired\n", LOG_START);

        // Set paused state
        bool previousPaused = this->paused;
        this->paused = paused;

        // Stop all running skills
        for (int i = 0; i != runningSkills.size(); ++i)  {
            runningSkills[i]->stop();
        }
        runningSkills.clear();

        // Set current/previous state
        currentState = stateMap[VID_STATE_PAUSED];
    }
    LOG("%s \t locks released\n", LOG_START);

    // Publish
    publishData(NULL, NULL);

    // Notify
    processSemaphore.notify();
}

void AutoVictimId::toggleRunDT() {
    LOG("%s Toggle Running DT to: %d\n", LOG_START, !onlyRunDT);
    onlyRunDT = !onlyRunDT;
    publishData(NULL, NULL);
}

void AutoVictimId::setExplorerMode(std::string mode) {
    config->explorerBehaviour = mode;
    publishData(NULL, NULL);
}

void AutoVictimId::execute() {
	LOG("%s execute()\n", LOG_START);

	while (operating) {
		// Wait on more data
	    //LOG("%s Waiting on process semaphore\n", LOG_START);
		processSemaphore.wait();

		// Lock processing
		Lock processLock(dtProcessMutex);

		// Run operation checks
		if (!operating) continue;
        if (paused) {
            publishData(NULL, NULL);
            continue;
        }

        // Do execution
        LOG("%s executeOnce\n", LOG_START);

		// Load data
		LOG("%s \t Load Data\n", LOG_START);
		loadData();

		// Execute State
		LOG("%s \t Execute State Machine\n", LOG_START);
		VIDSReturnDataPtr retData;
		retData = runStateMachine();

		// Stop/Start skills/behaviours
		if (!onlyRunDT) {
		    LOG("%s \t Set Active Skills\n", LOG_START);
		    setActiveSkills(retData);
		}

		// Special check for VID_STATE_VICTIM, to pause
		//LOG("%s \t Check next State for victim\n", LOG_START);
		if (retData->nextState == VID_STATE_VICTIM) {
		    setPaused(true);
		}

		// Set last time explored and do timeout if necessary
		//LOG("%s \t Timeout\n", LOG_START);
		if (retData->nextState == VID_STATE_INTIAL ||
		    retData->nextState == VID_STATE_EXPLORE ||
		    retData->nextState == VID_STATE_VICTIM) {
		    timeLastExplore = ros::Time::now();
		} else {
		    //LOG("%s \t Attempting timeout\n", LOG_START);
		    ros::Duration diff = ros::Time::now() - timeLastExplore;
		    if (fabs(diff.toSec()) > config->dtcTimeoutInvest) {
		        LOG("%s \t Investigate timeout - just explore\n", LOG_START);
		        retData->nextState = VID_STATE_EXPLORE;
		        timeLastExplore = ros::Time::now();
		    }
		}

		// Expire delayed holes
		// TODO - think about whether this is needed
		//expireDelayedHoles();

		// Publish Data
		LOG("%s \t Publish Data\n", LOG_START);
		publishData(data, retData);

		LOG("%s \t Loop\n", LOG_START);
	}
}

void AutoVictimId::loadData() {
    // Basic Data
    data = new VIDData();
    data->timeNow = ros::Time::now();
    data->config = config;
    data->camInspectPoint = Point3D();
    data->camInspectVector = Point2D();
    data->camInspectFrame = frameRobot;
    data->frameRobot = frameRobot;
    data->frameIcp = frameIcp;
    data->frameSlam = frameSlam;
    data->frameVictims = frameSlam;
    data->tfListener = tfListener;
    data->explorerClient = &explorerClient;
    data->followPathClient = &followPathClient;
    data->getPathClient = &getPathClient;
    data->lookAtClient = &lookAtClient;
    data->lookTrackClient = &lookTrackClient;
    data->snapPub = &snapPub;
    data->snapGetClient = &snapGetClient;
    data->snapUpdateClient = &snapUpdateClient;
    data->stowScanClient = &stowScanClient;

    // Load map data
    //LOG("%s Load Data Maps\n", LOG_START);
    loadDataMap();

    // Load Blobs
    //LOG("%s Load Data Blobs\n", LOG_START);
    loadDataBlobs();

    // Holes
    LOG("%s Load Data Holes\n", LOG_START);
    loadDataHoles();

    // Set additional fields
    data->processSBCentered = false;
    data->processSBRobotPos = Point3D(0,0,0);
    data->processSBIcpPos = Point3D(0,0,0);
    data->processSBSlamPos = Point3D(0,0,0);
    data->processSelectedBlob = NULL;
    data->blobCentreTimeout = false;
    data->delayedHole = NULL;
    data->camInspectPoint = Point3D(0,0,0);
    data->victimPosRobot = Point3D(0,0,0);
}

void AutoVictimId::loadDataMap() {
    // Load robot point
    Pose robotPose;         // In robot frame
    data->robotIcpPose = VIDUtil::transformPose(tfListener, frameRobot, frameIcp, data->timeNow, robotPose);
    data->robotSlamPose = VIDUtil::transformPose(tfListener, frameRobot, frameSlam, data->timeNow, robotPose);

    // Callback snaps - TODO is ok to reconnect each time?
    //if (!mapGetSnapsClient.isValid()) {
    //    mapGetSnapsClient = nh.serviceClient<crosbot_map::ListSnaps>(serviceMapGetSnapsName, true);
    //}
    //LOG("%s \t\t wait for snap service\n", LOG_START);
    bool foundService = ros::service::waitForService(serviceMapGetSnapsName, ros::Duration(DEFAULT_MAXWAIT_SERVICE));
    if (!foundService) {
        LOG("%s \t\t Failed to find Snaps List service: %s\n", LOG_START, serviceMapGetSnapsName.c_str());
    } else {
        LOG("%s \t\t Do service call\n", LOG_START);
        crosbot_map::ListSnapsRequest snapsRequest;
        crosbot_map::ListSnapsResponse snapsResponse;
        bool callbackData = mapGetSnapsClient.call(snapsRequest, snapsResponse);
        if (callbackData) {
            // Process additional data
            // Load marked victims
            // Load false victims
            // Load delayed holes - marked interesting points
            for (int i = 0; i != snapsResponse.snaps.size(); ++i) {
                crosbot_map::SnapMsg* snapMsg = &(snapsResponse.snaps[i]);

                if (snapMsg->type == Snap::VICTIM) {
                    if (snapMsg->status == Snap::CONFIRMED) {
                        Pose robotPose = snapMsg->robot;        // Slam Position when captured
                        Pose victimPose = snapMsg->pose;        // Robot relative

                        // Global slam point of victim
                        Pose globalVictimPose = Pose(robotPose.toTF() * victimPose.toTF());
                        data->foundVictims.push_back(globalVictimPose);
                    } else if (snapMsg->status == Snap::UNCONFIRMED ||
                               snapMsg->status == Snap::REJECTED) {
                        Pose robotPose = snapMsg->robot;        // Slam Position when captured
                        Pose victimPose = snapMsg->pose;        // Robot relative

                        // Global slam point of victim
                        Pose globalVictimPose = Pose(robotPose.toTF() * victimPose.toTF());
                        data->falseVictims.push_back(globalVictimPose);
                    }
                } else if (snapMsg->type == Snap::LANDMARK) {
                    data->qrCodes.push_back(new Snap(*snapMsg));
                } else if (snapMsg->type == Snap::HOLE) {
                    // Parse description
                    //sscanf(snapMsg->description.c_str(), "%d", &id);

                    Pose robotPose = snapMsg->robot;            // Slam Position when captured
                    Pose holePose = snapMsg->pose;              // Robot relative
                    Pose globalHolePose = Pose(robotPose.toTF() * holePose.toTF());

                    // Create Hole
                    Hole hole(snapMsg->id);
                    hole.frame = frameSlam;
                    hole.p = globalHolePose.position;
                    hole.perpend.x = globalHolePose.orientation.x;
                    hole.perpend.y = globalHolePose.orientation.z;

                    // Create Delayed Hole
                    bool inspected = snapMsg->status == Snap::CONFIRMED;
                    DelayedHolePtr delayedHole = new DelayedHole();
                    delayedHole->hole = hole;
                    delayedHole->inspected = inspected;
                    delayedHole->sideInspected = inspected;
                    delayedHole->timeAdded = data->timeNow;
                    LOG("%s \t Loading delayed point: id: %d slam: %s inspected: %d\n", LOG_START, hole.id, hole.p.toString().c_str(), inspected);
                    data->delayedHoles.push_back(delayedHole);
                }
            }
        }
    }
}

void AutoVictimId::loadDataBlobs() {
    // Connect to service
    if (!blobberClient.isValid()) {
        blobberClient = nh.serviceClient<thermal_blobber::BlobsService>(serviceBlobberName, true);
    }

    bool foundService = ros::service::waitForService(serviceBlobberName, ros::Duration(DEFAULT_MAXWAIT_SERVICE));
    if (!foundService) {
        //LOG("%s \t Failed to find blobber service: %s\n", LOG_START, serviceBlobber.c_str());
    } else {
        //LOG("%s \t Found Blobber\n", LOG_START);
        thermal_blobber::BlobsServiceRequest blobsRequest;
        thermal_blobber::BlobsServiceResponse blobsResponse;
        bool callbackData = blobberClient.call(blobsRequest, blobsResponse);
        if (callbackData) {
            // Load blob data type
            for(int i = 0; i != blobsResponse.blobs.blobs.size(); ++i) {
                BlobPtr blob = new Blob(blobsResponse.blobs.blobs[i]);
                data->blobs.push_back(blob);

                //LOG("%s \t\t Blob(%d) dist(%lf)\n", LOG_START, blob->id, blob->averageDistance);
            }
            data->blobsThreshold = blobsResponse.blobs.threshold;
            data->imgEncodedThermal = new Image(blobsResponse.blobs.encodedThermal);
            data->imgDistanceTh = new Image(blobsResponse.blobs.distance);
            data->timestampThermal = blobsResponse.blobs.thermalTimestamp;
            data->frameBlob = blobsResponse.blobs.blobFrame;
            //LOG("%s blobs size: %ld\n", LOG_START, data->blobs.size());

            // Sort blobs in increasing order of distance
            //LOG("%s Sort blobs\n", LOG_START);
            sort(data->blobs.begin(), data->blobs.end(), sortSwissBlobsReverse);
        } else {
            //LOG("\%s \t\t no data\n", LOG_START);
        }
    }
}

void AutoVictimId::loadDataHoles() {
    // Connect to service
    if (!holesClient.isValid()) {
        holesClient = nh.serviceClient<hole_detector::HoleDetectorService>(serviceHolesName, true);
    }

    bool foundService = ros::service::waitForService(serviceHolesName, ros::Duration(DEFAULT_MAXWAIT_SERVICE));
    if (!foundService) {
        //LOG("%s \t Failed to find holes service: %s\n", LOG_START, serviceHoles.c_str());
    } else {
        hole_detector::HoleDetectorServiceRequest holesRequest;
        hole_detector::HoleDetectorServiceResponse holesResponse;
        bool callbackData = holesClient.call(holesRequest, holesResponse);
        if (callbackData) {
            vector<Hole> holes;
            ros::Time timestamp;
            for(int i = 0; i != holesResponse.holes.size(); ++i) {
                holes.push_back(Hole(holesResponse.holes[i]));
                timestamp = holesResponse.pcTimestamp;
            }

            // Update delayed points from map data
            LOG("%s \t holes size: %Zd\n", LOG_START, holes.size());
            addDelayedHoles(holes, timestamp);
        }
    }
}

void AutoVictimId::addDelayedHoles(vector<Hole>& holes, ros::Time timestamp) {
    // Load robot position at time of capturing holes
    // Holes are in robot relative frame
    Pose robotPose;
    Pose robotIcpPose = VIDUtil::transformPose(tfListener, frameRobot, frameIcp, timestamp, robotPose);

    for(vector<Hole>::iterator it = holes.begin(); it != holes.end(); ++it) {
        //LOG("%s \t Attempting to add hole: %s\n", LOG_START, it->p.toString().c_str());
        addDelayedHole(*it, robotIcpPose, timestamp);
    }
}

void AutoVictimId::addDelayedHole(Hole hole, Pose robotIcpPose, ros::Time timestamp) {
    // Transform into slam frame
    Pose holeRobotPose;
    holeRobotPose.position = hole.p;
    Pose holeSlamPose = VIDUtil::transformPose(tfListener, hole.frame, frameSlam, timestamp, holeRobotPose);

    // Check if the hole should be added
    bool canAdd = true;
    for(unsigned int i = 0; i != data->delayedHoles.size(); i++) {
        if (holeSlamPose.position.distanceTo(data->delayedHoles[i]->hole.p) < config->dtcDelayedRadius) {
            canAdd = false;
        }
    }

    // Add if necessary
    if (canAdd) {
        // Update hole id
        hole.id = nextHoleId;
        nextHoleId++;

        // Publish new hole to the map
        SnapPtr holeSnap = new Snap();
        holeSnap->type = Snap::HOLE;
        holeSnap->id = hole.id;
        holeSnap->status = Snap::UNCONFIRMED;
        holeSnap->description = holeSlamPose.position.toString();
        holeSnap->robot = robotIcpPose;
        holeSnap->pose = holeRobotPose;
        snapPub.publish(holeSnap->toROS());

        // Create new hole at in slam frame
        Hole holeSlam(hole.id);
        holeSlam.frame = frameSlam;
        holeSlam.p = holeSlamPose.position;

        // Add to the current delayed holes
        LOG("%s \t Adding delayed point: robot: %s slam: %s\n", LOG_START, holeRobotPose.position.toString().c_str(),
                holeSlam.p.toString().c_str());
        data->delayedHoles.push_back(new DelayedHole(holeSlam, false, false, data->timeNow));
    }
}

void AutoVictimId::expireDelayedHoles() {
    // Filter
    /*deque<DelayedHolePtr> keep;
    ros::Time currentTime = data->timeNow;
    for(deque<DelayedHolePtr>::iterator it = delayedHoles.begin(); it != delayedHoles.end(); ++it) {
        DelayedHolePtr delayedHole = *it;
        ros::Duration diff = currentTime - delayedHole->timeAdded;
        if (fabs(diff.toSec()) < config->dtcLifeDelayed) {
            keep.push_back(*it);
        } else {
            //LOG("%s Expire delayed: w:(%.3lf, %.3lf, %.3lf)\n", LOG_START, delayedHole->hole.p.x, delayedHole->hole.p.y, delayedHole->hole.p.z);
        }
    }

    // Assign
    if (keep.size() != delayedHoles.size()) {
        delayedHoles.clear();
        delayedHoles = keep;
    }*/
}

VIDSReturnDataPtr AutoVictimId::runStateMachine() {
    Lock stateLock(dtStateMutex);
    VIDSReturnDataPtr retData = new VIDSReturnData();

    bool doneExecuteStates = false;
    while (!paused && !doneExecuteStates) {
        // Configure extra data
        LOG("%s \t\t previousState: %s\n", LOG_START, previousState.c_str());
        data->previousState = previousState;

        // Execute state
        LOG("%s \t\t Executing State: %s\n", LOG_START, currentState->getName().c_str());
        retData = currentState->execute(data);

        // Check for state change
        LOG("%s \t\t Next State: %s\n", LOG_START, retData->nextState.c_str());
        if (currentState->getName() == retData->nextState) {
            doneExecuteStates = true;
        } else {
            doneExecuteStates = false;
        }

        // Update next state
        previousState = currentState->getName();
        currentState = stateMap[retData->nextState];
    }

    return retData;
}

void AutoVictimId::setActiveSkills(VIDSReturnDataPtr retData) {
    Lock skillLock(dtSkillMutex);

    if (paused || !operating) return;

    // Sort skills by start/stop
    deque<VicIdSkillPtr> startSkills = retData->skills;
    deque<VicIdSkillPtr> stopSkills;

    // Iterate through running skills and check for equality
    LOG("%s \t\t Running skills: %Zd\n", LOG_START, runningSkills.size());
    for (int i = 0; i != runningSkills.size(); ++i) {
        VicIdSkillPtr runSk = runningSkills[i];
        bool match = false;
        deque<VicIdSkillPtr>::iterator matchIt;
        for (deque<VicIdSkillPtr>::iterator it = startSkills.begin();
                it != startSkills.end(); ++it) {
            if ((*it)->equals(*runSk)) {
                match = true;
                matchIt = it;
            }
        }

        if (match) {
            LOG("%s \t\t Skill (%s) already running\n", LOG_START, (*matchIt)->getName().c_str());
            if (!(*matchIt)->getRerun()) {
                startSkills.erase(matchIt);
            } else {
                LOG("%s \t\t\t re-start it\n", LOG_START);
            }
        } else {
            stopSkills.push_back(runSk);
        }
    }

    // Stop skills to be stopped
    //LOG("%s \t\t To Stop skills: %ld\n", LOG_START, stopSkills.size());
    for(deque<VicIdSkillPtr>::iterator it = stopSkills.begin(); it != stopSkills.end(); ++it) {
        LOG("%s \t\t Stopping Skill: %s\n", LOG_START, (*it)->getName().c_str());
        (*it)->stop();
    }

    // Start skills to be started
    //LOG("%s \t\t To Start skills: %ld\n", LOG_START, startSkills.size());
    for(deque<VicIdSkillPtr>::iterator it = startSkills.begin(); it != startSkills.end(); ++it) {
        LOG("%s \t\t Starting Skill: %s\n", LOG_START, (*it)->getName().c_str());
        (*it)->start();
    }

    // Record running skills
    runningSkills = retData->skills;
}

void AutoVictimId::publishData(VIDDataPtr data, VIDSReturnDataPtr retData) {
    victim_id::VictimIdDataMsg msg;

    // General data
    msg.paused = paused;
    if (paused) msg.state = previousState;
    msg.onlyRunDT = onlyRunDT;
    msg.wallFollowMode = config->explorerBehaviour;

    // retData
    if (retData != NULL) {
        msg.state = retData->nextState;
    }

    // Victim Data
    if (data != NULL) {
        msg.blobDistance = (data->processSelectedBlob != NULL) ? data->processSelectedBlob->averageDistance : 0;
        msg.blobPosition = data->processSBRobotPos.toROS();
        msg.blobsCount = data->blobs.size();
        msg.blobsThreshold = data->blobsThreshold;
    }

    vicDataPub.publish(msg);
}

} // namespace casualty


