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

#include "victim_id/states/explore.hpp"

#include "victim_id/util.hpp"
#include "victim_id/states/investigate.hpp"
#include "victim_id/skills/skillExplore.hpp"

#define LOG_START   "VIDSExplore :: "

#include <limits>

namespace casualty {

VIDSExplore::VIDSExplore() :
    VIDState(VID_STATE_EXPLORE)
{}

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

    // Choose state - default to staying the same
    retData->nextState = VID_STATE_EXPLORE;

    // Process blobs
    //LOG("%s \t\t Processing Blobs\n", LOG_START);
    processBlobs(data);

    if (data->processSelectedBlob != NULL) {
        // Found a blob to look at
        retData->nextState = VID_STATE_INVESTIGATE;
    } else {
        // Look for holes to investigate
        int index;
        bool sideInspected;
        if (delayedInvestigation(data, index, sideInspected)) {
            // Found hole to investigate
            retData->nextState = VID_STATE_INVESTIGATE;
            data->delayedHole = data->delayedHoles[index];
            LOG("%s \t\t Delayed investigate point: %s\n", LOG_START, data->delayedHole->hole.p.toString().c_str());
        }
    }

    // TODO HACK - remove
    retData->nextState = VID_STATE_EXPLORE;

    // Setup skills
    if (retData->nextState == VID_STATE_EXPLORE) {
        VIDSkillExplorePtr skExplore = new VIDSkillExplore();
        skExplore->explorerClient = data->explorerClient;
        skExplore->stowScanClient = data->stowScanClient;
        skExplore->mode = data->config->explorerBehaviour;
        retData->skills.push_back(skExplore);
    }

    return retData;
}

bool VIDSExplore::delayedInvestigation(VIDDataPtr data, int& investIndex, bool& sideInspected) {
    bool havePoint = false;
    bool sideInspectInternal = false;

    // Got any holes?
    if (data->delayedHoles.size() == 0) return false;

    // Find closest delayed inspection point
    LOG("%s \t\t Delayed: Robot World Pos: %s Num Delayed Points: %ld\n", LOG_START, data->robotSlamPose.position.toString().c_str(), data->delayedHoles.size());
    double minDist = std::numeric_limits<double>::max();
    for(unsigned int i = 0; i < data->delayedHoles.size(); i++) {
        DelayedHolePtr delayedHole = data->delayedHoles[i];
        Point3D holeP = (*delayedHole).hole.p;
        double pDist = holeP.distanceTo(data->robotSlamPose.position);
        bool choose = false;

        if (pDist < minDist &&
            pDist <= data->config->dtcTravelDelayed &&
            !alreadyFoundVic(data, holeP)) {
            if (!(*delayedHole).inspected) {
                if (!pointBehindRobot(data, holeP, data->frameSlam, data->timeNow)) {
                    //LOG("%s \t Chosen point: %s dist: %.3lf\n", LOG_START, holeP.toString().c_str(), pDist);
                    choose = true;
                }
            } else if (!(*delayedHole).sideInspected) {
                if (pointBesideRobot(data, holeP, data->frameSlam, data->timeNow)) {
                     //LOG("%s dinvest: not side inspectecd\n", LOG_START);
                     choose = true;
                     sideInspectInternal = true;
                }
            }
        }

        if (choose) {
            havePoint = true;
            investIndex = i;
            sideInspected = sideInspectInternal;
            minDist = pDist;
        } else {
            //DelayedPoint p = dtDelayedPos[i];
            //Point3D pr = lookAt->convertPointFrame(p.p, LAF_WORLD, LAF_ROBOT);
            //Point2D vt = skillSE->getVoronoiTarget();
            //LOG("%s dtDelayedPos[%d] r:(%.3lf,%.3lf,%.3lf)\n", LOG_START, i, pr.x, pr.y, pr.z);
            //LOG("%s inspected:%d\n", LOG_START, p.inspected);
            //LOG("%s pDist: %.3lf\n", LOG_START, pDist);
            //LOG("%s onpath: %d\n", LOG_START, pointOnRobotPath(p.p, LAF_WORLD));
            //LOG("%s explore p: (%.3lf,%.3lf)\n", LOG_START, vt.x, vt.y);
        }
    }

    return havePoint;
}

} // namespace casualty

