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

#include "victim_id/states/investigate.hpp"

#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/lostHotSpot.hpp"
#include "victim_id/states/victim.hpp"
#include "victim_id/skills/skillLookAt.hpp"

#include <crosbot_map/tag.hpp>

#define LOG_START   "VIDSPaused :: "

using std::string;

namespace casualty {

VIDSInvestigate::VIDSInvestigate() :
    VIDState(VID_STATE_INVESTIGATE)
{}

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

    // Default to exploring
    retData->nextState = VID_STATE_EXPLORE;
    bool ignoreCentre = false;

    // Check for if we have just changed to this state
    bool doBlobProcess = true;
    bool returnDrive = false;
    if (continued) {
        // If we've continued then re-load all the blob data

    } else {
        if (data->previousState == VID_STATE_CAMERA_INSPECT) {
        } else if (data->previousState == VID_STATE_CENTRE_BLOB) {
            ignoreCentre = data->blobCentreTimeout;
        } else if (data->previousState == VID_STATE_DRIVE_TO_BLOB) {
            returnDrive = true;
        } else if (data->previousState == VID_STATE_EXPLORE) {
            doBlobProcess = false;
        } else if (data->previousState == VID_STATE_LOST_HOT_STOP) {
        } else if (data->previousState == VID_STATE_VICTIM) {
        }
    }
    if (doBlobProcess) processBlobs(data);

    // Investigate data
    Point3D driveToPoint;
    string driveToFrame;

    if (data->processSelectedBlob != NULL) {
        // Have we got a blob to look at?
        bool doCentre = false;
        bool doVictim = false;
        bool doDrive = false;

        if (!ignoreCentre && !data->processSBCentered) {
            doCentre = true;
        } else if (data->processSelectedBlob->averageDistance <= data->config->dtcDistVictimHS) {
            doVictim = true;
        } else if (returnDrive) {
            doVictim = true;
        } else {
            doDrive = true;
        }

        if (doCentre) {
            LOG("%s \t\t\t centre the blob\n", LOG_START);
            retData->nextState = VID_STATE_CENTRE_BLOB;
        } else if (doVictim) {
            LOG("%s \t\t\t we have a victim!!!!\n", LOG_START);
            data->victimPosRobot = data->processSBRobotPos;
            retData->nextState = VID_STATE_VICTIM;
        } else if (doDrive) {
            LOG("%s \t\t\t get closer to a point\n", LOG_START);
            retData->nextState = VID_STATE_DRIVE_TO_BLOB;
        } else {
            LOG("%s \t\t\t Lost what we were looking at so just explore\n", LOG_START);
            retData->nextState = VID_STATE_EXPLORE;
        }
    } else if (data->delayedHole != NULL) {
        // Have we got a delayed hole to look at?
        retData->nextState = VID_STATE_CAMERA_INSPECT;
        data->camInspectPoint = data->delayedHole->hole.p;
        data->camInspectVector = data->delayedHole->hole.perpend;
        data->camInspectFrame = data->frameSlam;
    } else {
        LOG("%s \t\t\t Lost what we were looking at so just explore\n", LOG_START);
        retData->nextState = VID_STATE_EXPLORE;
    }

    // Update delayed holes
    if (retData->nextState == VID_STATE_CAMERA_INSPECT) {
        updatedDelayedHoles(data, data->camInspectPoint, data->camInspectFrame, data->config->dtcDelayedRadius, true);
    } else if (retData->nextState == VID_STATE_CENTRE_BLOB) {
    } else if (retData->nextState == VID_STATE_DRIVE_TO_BLOB) {
        updatedDelayedHoles(data, data->processSBSlamPos, data->frameSlam, data->config->dtcDelayedRadius, true);
    } else if (retData->nextState == VID_STATE_EXPLORE) {
    } else if (retData->nextState == VID_STATE_VICTIM) {
        updatedDelayedHoles(data, data->victimPosRobot, data->frameSlam, data->config->dtcVictimRadius, true);
    }

    // Setup Skills
    if (retData->nextState == VID_STATE_INVESTIGATE) {
        LOG("%s **** WTF??\n", LOG_START);
        retData->nextState = VID_STATE_EXPLORE;
    }

    return retData;
}

void VIDSInvestigate::updatedDelayedHoles(VIDDataPtr data, Point3D point, string frame, double radius, bool sideInspected) {
    Point3D p = point;
    if (frame != data->frameSlam) {
        p = VIDUtil::transformPoint(data->tfListener, frame, data->frameSlam, data->timeNow, point);
    }
    for(int i = 0; i < data->delayedHoles.size(); i++) {
        DelayedHolePtr delayedHole = data->delayedHoles[i];
        //LOG("%s p(%s) delayedHole(%s) dist(%.3lf) radius(%.3lf)\n", LOG_START, p.toString().c_str(), delayedHole->hole.p.toString().c_str(), delayedHole->hole.p.distanceTo(p), radius);
        if (delayedHole->hole.p.distanceTo(p) < radius) {
            delayedHole->inspected = true;
            if (sideInspected) delayedHole->sideInspected = true;

            // Publish update to the map
            crosbot_map::ModifySnapRequest request;
            request.id.data = delayedHole->hole.id;
            request.type.data = Snap::HOLE;
            request.status.data = Snap::CONFIRMED;
            updateSnap(data->snapUpdateClient, request);
        }
    }
}

} // namespace casualty

