/*
 * state.cpp
 *
 *  Created on: 18/06/2013
 *      Author: timothyw
 */

#include "victim_id/state.hpp"

#include <crosbot/utils.hpp>
#include <cmath>
#include <victim_id/util.hpp>

#define DEFAULT_MAXWAIT_SERVICE     2.0
#define DEFAULT_MAXWAIT_TRANSFORM   2.0
#define LOG_START                   "VIDState :: "

using std::deque;
using std::string;

namespace casualty {

VIDSReturnDataPtr VIDState::execute(VIDDataPtr data) {
    continued = data->previousState == name;
    return doExecute(data);
}

void VIDState::updateSnap(ros::ServiceClient* snapUpdateClient, crosbot_map::ModifySnapRequest& request) {
    bool foundService = ros::service::waitForService(snapUpdateClient->getService(),
            ros::Duration(DEFAULT_MAXWAIT_SERVICE));
    if (!foundService) {
        LOG("%s \t Failed to find Snap Update service: %s\n", LOG_START, snapUpdateClient->getService().c_str());
    } else {
        //LOG("%s \t Found Snap Update\n", LOG_START);
        //LOG("%s Update snap: id: %d type: %d\n", LOG_START, request.id.data, request.type.data);
        crosbot_map::ModifySnapResponse response;
        bool callbackData = snapUpdateClient->call(request, response);
        if (!callbackData) {
            LOG("%s callback update snap failed\n", LOG_START);
        } else {
            //LOG("%s update snap failed\n", LOG_START);
        }
    }
}

bool VIDState::alreadyFoundVic(VIDDataPtr data, Point3D pos) {
    bool alreadyFound = false;

    for(deque<Pose>::iterator it = data->foundVictims.begin();
            !alreadyFound && it != data->foundVictims.end(); ++it) {
        //LOG("%s Victimdist(%lf,%lf,%lf): %lf\n", LOG_START, (*it).x, (*it).y, (*it).z, pos.distance(*it));
        alreadyFound = pos.distanceTo(it->position) < data->config->dtcVictimRadius;
    }
    for(deque<Pose>::iterator it = data->falseVictims.begin();
                !alreadyFound && it != data->falseVictims.end(); ++it) {
        alreadyFound = pos.distanceTo(it->position) < data->config->dtcFalsePositiveRadius;
    }

    return alreadyFound;
}

void VIDState::processBlobs(VIDDataPtr data) {
    if (data->blobs.size() > 0) {
        // Sort blobs descending on order
        // Blobs are already sorted

        // Have timestamp relative robot/icp position
        Pose robotPose;         // In robot frame
        Pose robotIcpPose = VIDUtil::transformPose(data->tfListener, data->frameRobot, data->frameIcp, data->timestampThermal, robotPose);
        Pose robotSlamPose = VIDUtil::transformPose(data->tfListener, data->frameRobot, data->frameSlam, data->timestampThermal, robotPose);
        //Pose robotIcpPose = VIDUtil::transformPose(data->tfListener, data->frameRobot, data->frameIcp, data->timeNow, robotPose);
        //Pose robotSlamPose = VIDUtil::transformPose(data->tfListener, data->frameRobot, data->frameSlam, data->timeNow, robotPose);

        // Select the closest blob matching the desired properties (ie the one we want to deal with)
        //      - Must be below celling
        //      - Must not be previously denoted as a victim
        //      - Must be outside radius of robot
        //      - Must be within consideration distance
        //      - Must be within a hole
        //      - If not within a hole, must be near the floor
        LOG("%s \t\t\t Select next blob to process\n", LOG_START);
        data->processSelectedBlob = NULL;
        for (unsigned int blobIndex = 0; blobIndex != data->blobs.size(); blobIndex++) {
            BlobPtr blob = data->blobs[blobIndex];

            // Load blob properties
            // TODO Check correct calculation
            // Robot Pos is robot relative as of the timestamp
            //  - assume that the current icp/slam position of the robot are close to the timestamp of the captured blobs
            Point3D robotPos = VIDUtil::transformPoint(data->tfListener, data->frameBlob, data->frameRobot, data->timestampThermal, blob->p);
            Pose icpPose = Pose(robotIcpPose.toTF() * Pose(robotPos, Quaternion()).toTF());
            Pose slamPose = Pose(robotSlamPose.toTF() * Pose(robotPos, Quaternion()).toTF());
            Point3D icpPos = icpPose.position;
            Point3D slamPos = slamPose.position;
            double robotCentDist = robotPos.distanceTo(Point3D(0, 0, 0));
            LOG("%s \t\t\t Blob[%d] robotpos: %s thermalpos: %s dist: %.3lf\n", LOG_START,
                    blobIndex, robotPos.toString().c_str(), blob->p.toString().c_str(),
                    blob->averageDistance);

            // Should the blob be selected
            bool selectBlob = false;

            // Check properties
            // Slam/ICP position is always 1 metre above the ground
            if (blob->averageDistance > 0 &&
                robotPos.z < data->config->dtcBlobCelling &&
                robotCentDist > data->config->dtcRobotRadius &&
                !alreadyFoundVic(data, slamPos)) {

                LOG("%s \t\t\t Maybe Select\n", LOG_START);
                LOG("%s \t\t\t\t blob pos: %s dist: %.3lf\n",
                        LOG_START, blob->p.toString().c_str(), blob->averageDistance);
                LOG("%s \t\t\t\t r: %s s: %s\n", LOG_START,
                            robotPos.toString().c_str(), slamPos.toString().c_str());

                // Decide if we should select blob or delay
                if (blob->averageDistance < data->config->dtcDistConsiderHS) {
                    if (robotPos.z < data->config->dtcBlobDefHeight) {
                        selectBlob = true;
                    } else if (pointWithinHole(data, slamPos)) {
                        selectBlob = true;
                    }
                //} else if (!pointOnRobotPath(robotPos, LAF_WORLD)) {
                //    selectBlob = true;
                //} else if (skillSE->getBehaviour() == GotoWaypoint) {
                //    selectBlob = true;
                //} else if (sb->averageDist < dtcDistConsiderHS) {
                //    addDelayed(worldPos);
                }
            } else {
                LOG("%s \t\t\t Not select\n", LOG_START);
                LOG("%s \t\t\t\t slam pos: %s cel:%.3lf\n", LOG_START, slamPos.toString().c_str(), data->config->dtcBlobCelling);
                LOG("%s \t\t\t\t blob dist: %.3lf rad:%.3lf\n", LOG_START, robotPos.distanceTo(Point3D(0, 0, 0)), data->config->dtcRobotRadius);
                LOG("%s \t\t\t\t already found: %d\n", LOG_START, alreadyFoundVic(data, slamPos));
            }

            // If blob is selected, save necessary parameters
            if (selectBlob) {
                data->processSelectedBlob = blob;
                data->processSBRobotPos = robotPos;
                data->processSBIcpPos = icpPos;
                data->processSBSlamPos = slamPos;
                data->processSBCentered = blobIsCentered(data, data->processSelectedBlob);

                LOG("%s \t\t\t SB Pos: r: %s w: %s cent:%d\n", LOG_START,
                        data->processSBRobotPos.toString().c_str(),
                        data->processSBSlamPos.toString().c_str(),
                        data->processSBCentered);
            } else {
                LOG("%s \t\t\t Failed criteria: withinRange: %d belowDefHeight: %d withinHole: %d\n",
                        LOG_START,
                        blob->averageDistance < data->config->dtcDistConsiderHS,
                        robotPos.z < data->config->dtcBlobDefHeight,
                        pointWithinHole(data, slamPos));
            }
        }
    // NO Blobs
    } else {
        LOG("%s \t\t\t No Blobs to process\n", LOG_START);
        // Have we lost a spot we were going to?
        //if (activeChoice.type == AC_INVEST_POS) {
        //    choice.type = AC_LOST_HS;
        //    choice.pFrame = LAF_PTZ;
        //    choice.camInspectP = lookAt->convertPointFrame(activeChoice.camInspectP, activeChoice.pFrame, LAF_PTZ);
        //} else if (activeChoice.type == AC_LOST_HS) {
        //    choice = activeChoice;
        //}
    }
}

bool VIDState::blobIsCentered(VIDDataPtr data, BlobPtr blob) {
    ImagePtr image = data->imgEncodedThermal;
    double centreFraction = data->config->dtcCentreFraction;
    Point2D pixel = blob->centre;

    // Define bounding box
    int xAdjust = image->width * (centreFraction / 2);
    int yAdjust = image->height * (centreFraction / 2);
    int centerX = image->width / 2;
    int centerY = image->height / 2;
    int centreT = centerX - xAdjust;
    int centreR = centerY + yAdjust;
    int centreB = centerX + xAdjust;
    int centreL = centerY - yAdjust;

    bool centered = false;
    if (pixel.x > centreL &&
        pixel.x < centreR &&
        pixel.y > centreT &&
        pixel.y < centreB) {
        centered = true;
    }

    return centered;
}

bool VIDState::pointWithinHole(VIDDataPtr data, Point3D pointSlam) {
    bool within = false;
    for(deque<DelayedHolePtr>::iterator it = data->delayedHoles.begin();
        !within && it != data->delayedHoles.end(); ++it) {
        //LOG("%s bwh dist from hole: %.3lf\n", LOG_START, blob.distance(it->hole.p));
        if (pointSlam.distanceTo((*it)->hole.p) < data->config->dtcBlobHoleDist) {
            //LOG("%s\twithin!!\n", LOG_START);
            within = true;
        }
    }
    return within;
}

bool VIDState::pointBesideRobot(VIDDataPtr data, Point3D pos, string pointFrame, ros::Time timestamp) {
    Point3D posRobot = VIDUtil::transformPoint(data->tfListener, pointFrame, data->frameRobot, timestamp, pos);
    Point2D pos2D(posRobot.x, posRobot.y);
    Point2D ahead(0, 1);
    double angle = abs(ahead.angleTo(pos2D));
    //LOG("%s PBR: p:(%.3lf, %.3lf) angle:%.3lf (%.3lf, %.3lf)\n", LOG_START, posRobot.x, posRobot.y, RAD2DEG(angle), RAD2DEG(dtcBesideMin), RAD2DEG(dtcBesideMax));
    return (angle > data->config->dtcBesideMin && angle < data->config->dtcBesideMax);
}

bool VIDState::pointBehindRobot(VIDDataPtr data, Point3D pos, string pointFrame, ros::Time timestamp) {
    Point3D posRobot = VIDUtil::transformPoint(data->tfListener, pointFrame, data->frameRobot, timestamp, pos);
    double length = data->config->robotLength;
    return (posRobot.x < 0 - (length / 2));
}

} // namespace casualty
