/*
 * state.hpp
 *
 *  Defines a state
 *
 *  Created on: 16/06/2013
 *      Author: timothyw
 */

#ifndef VICTIMID_STATE_HPP_
#define VICTIMID_STATE_HPP_

#define VID_STATE_NO_STATE  "state_no_state"

#include "victim_id/class.hpp"
#include "victim_id/data.hpp"
#include "victim_id/skill.hpp"

#include <crosbot/utils.hpp>
#include <crosbot_map/ModifySnap.h>

#include <deque>

namespace casualty {

// Here for necessary forward reference
class VIDState;
typedef crosbot::Handle<VIDState> VIDStatePtr;

/**
 * Return data structure for victim id states
 */
class VIDSReturnData : public crosbot::HandledObject {
public:
    std::string nextState;                  // State to switch to - can be the current state
    std::deque<VicIdSkillPtr> skills;       // Next set of skills to activate
};
typedef crosbot::Handle<VIDSReturnData> VIDSReturnDataPtr;

/**
 * Class for implementing victim id states
 */
class VIDState : public crosbot::HandledObject {
public:
    VIDState(std::string name) :
        name(name),
        continued(false)
    {};
    ~VIDState() {};

    inline std::string getName() { return name; };

    // Run to allows states to subscribe/publish topics
    virtual void init(ros::NodeHandle& nh, VIDConfigPtr config) {};

    /**
     * Execute the state
     */
    VIDSReturnDataPtr execute(VIDDataPtr data);

    // Update the given snap
    void updateSnap(ros::ServiceClient* snapUpdateClient, crosbot_map::ModifySnapRequest& request);

protected:
    // Unique indentifier for the state, which will act as it's reference
    std::string name;
    // If this state is continued from before
    bool continued;

    // Internal execute method, called by execute()
    virtual VIDSReturnDataPtr doExecute(VIDDataPtr data) = 0;

    // Processing methods
    // Have we already found the victim
    bool alreadyFoundVic(VIDDataPtr data, Point3D pos);
    // Process Blobs
    void processBlobs(VIDDataPtr data);

    // Useful methods
    // True if the centre of the blob is within the bounding box defined by the centreFraction
    bool blobIsCentered(VIDDataPtr data, BlobPtr blob);
    // True if the given blobs is within blobHoleDist of any hole
    bool pointWithinHole(VIDDataPtr data, Point3D blob);
    // True if the given point is beside the robot
    bool pointBesideRobot(VIDDataPtr data, Point3D pos, std::string pointFrame, ros::Time timestamp);
    // True if the given point is behind the robot
    bool pointBehindRobot(VIDDataPtr data, Point3D pos, std::string pointFrame, ros::Time timestamp);
};

} // namespace casualty

#endif /* VICTIMID_STATE_HPP_ */
