/*
 * victimId.hpp
 *
 *  Created on: 14/06/2013
 *      Author: timothyw
 */

#ifndef VICTIMID_VICTIMID_HPP_
#define VICTIMID_VICTIMID_HPP_

#include <ros/ros.h>

#include "victim_id/class.hpp"

#include <crosbot/config.hpp>
#include <crosbot/handle.hpp>
#include <crosbot/thread.hpp>

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

#include <deque>
#include <map>
#include <vector>

using namespace crosbot;
using std::deque;
using std::map;
using std::string;
using std::vector;

namespace casualty {

class AutoVicPing : public Thread, public HandledObject {
public:
    AutoVicPing(std::string name, AutoVictimId* autoVictimId, double frequency);
    ~AutoVicPing() {};

    void run();
    void start();
    void stop();
private:
    bool operating;
    AutoVictimId* autoVictimid;
    double frequency;
};
typedef Handle<AutoVicPing> AutoVicPingPtr;

class AutoVictimId : public Thread, public HandledObject {
public:
    AutoVictimId();
    ~AutoVictimId() {};

    // Thread run()
    void run();
    // Stop the victim id completely
    void stop();
    // Pause/unpause victim id - saving state
    void setPaused(bool paused);
    // Toggles if the DT should be run
    void toggleRunDT();
    // Set stateless explorer mode
    void setExplorerMode(std::string mode);

    // Load information from configuration
    void configure(ConfigElementPtr config);
    // Perform any tasks to be done after config load, but not on each run
    void init(ros::NodeHandle& nh);

    // Notify semaphore
    inline void notifySemaphore() {
        processSemaphore.notify();
    };

protected:
    // ROS elements
    ros::NodeHandle nh;
    ros::ServiceClient blobberClient;
    ros::ServiceClient holesClient;
    ros::ServiceClient mapGetSnapsClient;
    ros::ServiceClient explorerClient;
    ros::ServiceClient followPathClient;
    ros::ServiceClient getPathClient;
    ros::ServiceClient lookAtClient;
    ros::ServiceClient lookTrackClient;
    ros::ServiceClient snapGetClient;
    ros::ServiceClient snapUpdateClient;
    ros::ServiceClient stowScanClient;
    ros::Publisher snapPub;
    ros::Publisher vicDataPub;
    tf::TransformListener* tfListener;
    //ros::Publisher depthProcPubColour, depthProcPubDepth;
    //ros::Subscriber depthProcSub;

    // Processing controls
    bool operating;                         // Victim id is in operation
    bool paused;                            // Is paused - but not to be shutdown
    bool onlyRunDT;                         // Only run the decision tree
    AutoVicPingPtr ping;                    // Ping at the required frequency
    Semaphore processSemaphore;             // Hit when the execute loop should re-process

    // Config parameters
    VIDConfigPtr config;                    // Victim Id config parameters
    std::string frameRobot;                 // Robot Frame
    std::string frameIcp;                   // ICP Frame
    std::string frameSlam;                  // Slam Frame
    double frequency;                       // Frequency of re-run
    std::string pubSnapsName;               // Name of topic to publish new snaps
    std::string pubVicDataName;             // Name of topic to publish victim data
    std::string serviceExplorerName;        // Name of topic to publish explorer commands
    std::string serviceBlobberName;         // Name of the blobber service
    std::string serviceFollowPathName;      // Name of follow path service
    std::string serviceGetPathName;         // Name of get path service
    std::string serviceHolesName;           // Name of the holes service
    std::string serviceLookAtName;          // Name of topic to publish camera looking commands
    std::string serviceLookTrackName;       // Name of topic to publish camera tracking commands
    std::string serviceMapGetSnapsName;     // Name of service to get snaps
    std::string serviceStowScanName;        // Name of service to stow arm and scan the head
    std::string serviceSnapGetName;         // Name of service to get a snap
    std::string serviceSnapUpdateName;      // Name of service to update a snap

    // Concurrency
    Mutex dtProcessMutex;                   // Process decision tree
    Mutex dtSkillMutex;                     // Locked when modifying skills
    Mutex dtStateMutex;                     // Locked when choosing the next state

    // States
    map<string, VIDStatePtr> stateMap;      // Map of string to state for easy lookup
    VIDStatePtr currentState;               // Current state
    std::string previousState;              // Previous state

    // Data
    VIDDataPtr data;                        // Current iteration of data
    ros::Time timeLastExplore;              // Last time state_explore was run
    int nextHoleId;                         // Counter for the next hole id

    // Behaviours/Skills
    deque<VicIdSkillPtr> runningSkills;     // Currently running skills

    // Execute Loop
    void execute();

    // Loads a new copy of all the data
    void loadData();
    // Loads data from the slam map
    void loadDataMap();
    // Load blobs
    void loadDataBlobs();
    // Load holes
    void loadDataHoles();
    // Add delayed holes (from hole detector)
    void addDelayedHoles(vector<Hole>& holes, ros::Time timestamp);
    // Add a single hole (from the hole detector)
    void addDelayedHole(Hole hole, Pose robotIcpPose, ros::Time timestamp);
    // Expire Delayed Holes
    void expireDelayedHoles();
    // Run the state machine
    VIDSReturnDataPtr runStateMachine();
    // Set the active skills
    void setActiveSkills(VIDSReturnDataPtr retData);
    // Publish data for panel
    void publishData(VIDDataPtr data, VIDSReturnDataPtr retData);
};

} // namespace casualty

#endif /* VICTIMID_VICTIMID_HPP_ */
