/*
 * data.hpp
 *
 *  Created on: 16/06/2013
 *      Author: timothyw
 */

#ifndef VICTIMID_DATA_HPP_
#define VICTIMID_DATA_HPP_

#include "victim_id/class.hpp"

#include <crosbot_map/tag.hpp>
#include <hole_detector/holeDetector.h>
#include <thermal_blobber/blob.h>

#include <deque>
#include <vector>

namespace casualty {

// Holes to inspect as the robot nears them
class DelayedHole : public HandledObject {
public:
    Hole hole;
    bool inspected;
    bool sideInspected;
    ros::Time timeAdded;

    DelayedHole() :
        hole(0, Point3D(), Point3D(), Point3D()),
        inspected(true),
        sideInspected(true),
        timeAdded(ros::Time::now())
    {};
    DelayedHole(Hole hole, bool inspected, bool sideInspected, ros::Time timeAdded) :
        hole(hole),
        inspected(inspected),
        sideInspected(sideInspected),
        timeAdded(timeAdded)
    {};

};
typedef Handle<DelayedHole> DelayedHolePtr;

class VIDConfig : public crosbot::HandledObject {
public:
    double dtcBesideMin;                    // Minimum angle 'beside' the robot
    double dtcBesideMax;                    // Maximum angle 'beside' the robot
    double dtcBlobCelling;                  // Maximum height (z) in robot space for a valid blob
    double dtcBlobDefHeight;                // Height when a blob is defintely should be looked at
    double dtcBlobHoleDist;                 // Distance between blob & hole centres
    double dtcCameraInspectTimeout;         // Timeout for looking at things with the camera
    double dtcCentreFraction;               // Fraction defining the 'centre' bounding box
    double dtcCentreTimeout;                // Timeout for centering blobs
    double dtcDistConsiderHS;               // Distance at which to consider Hot Spots
    double dtcDistVictimHS;                 // Distance at which hot spot is a victim
    double dtcDelayedMaxAngle;              // Max allowable angle for inspecting between norm & robot pos
    double dtcDelayedRadius;                // Distance in which delayed raduis points can be added
    int    dtcDeviatePathAngle;             // Minimum angle required to deviate from current path
    double dtcDriveToTimeout;               // Timeout for the drive to skill
    double dtcFalsePositiveRadius;          // Radius for false positives
    double dtcInitialDelayedTime;           // Delay time before victims are looked for initially
    double dtcLifeDelayed;                  // Life of the delayed points
    double dtcLostHotSpotTimeout;           // Timeout for loosing the hot spot
    double dtcQrCodeRange;                  // Range to match qr code to victim
    double dtcRerunTimeout;                 // Rerun timeout of the tree in microseconds
    double dtcRerunTimeoutDTOnly;           // Rerun timeout of the tree in microseconds (when in DT Only mode)
    double dtcRobotRadius;                  // Radius around robot (0,0,0) the robot base is in
    double dtcTimeoutInvest;                // Timeout on investigate position autonomy
    double dtcTravelDelayed;                // Maximum distance allowable to travel to delayed hole
    double dtcVictimRadius;                 // Radius of a given victim another victim cannot be in

    std::string explorerBehaviour;          // Stateless explorer mode

    // Other parameters - not dtc values
    double robotLength;                     // Length of the robot

    // ROS Subscribers
    std::string subExplorerName;            // Name of topic to subscribe to explorer
    std::string subLookAtName;              // Name of topic to subscribe to camera look at
    std::string subColourImgName;           // Name of the colour img topic
    std::string subDistImgName;             // Name of the dist img topic
};
typedef crosbot::Handle<VIDConfig> VIDConfigPtr;

class VIDData : public crosbot::HandledObject {
public:
    // Basic data & Config
    VIDConfigPtr config;                    // Loaded config
    ros::Time timeNow;                      // Now time created when data instantiated
    tf::TransformListener* tfListener;      // transform listener
    std::string frameRobot;                 // string identifying the robot frame
    std::string frameIcp;                   // ICP frame
    std::string frameSlam;                  // Slam frame

    // ROS Publishers
    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;

    // Robot Information
    Pose robotIcpPose;                      // position in icp frame
    Pose robotSlamPose;                     // position in slam frame

    // Existing useful information
    std::deque<Pose> foundVictims;          // Victims found (from slam map)
    std::deque<Pose> falseVictims;          // Victims falsely identified (from slam map)
    std::string frameVictims;

    // Blobber
    std::vector<BlobPtr> blobs;
    int blobsThreshold;
    crosbot::ImagePtr imgEncodedThermal;
    crosbot::ImagePtr imgDistanceTh;
    ros::Time timestampThermal;
    std::string frameBlob;

    // Hole detector
    std::deque<DelayedHolePtr> delayedHoles;

    // QR Codes
    std::deque<SnapPtr> qrCodes;

    // DT processing/state information
    std::string previousState;

    // Data that is set by processing of states
    BlobPtr processSelectedBlob;            // Selected blob to look at
    Point3D processSBRobotPos;              // Robot position of selected blob
    Point3D processSBIcpPos;                // ICP position of selected blob
    Point3D processSBSlamPos;               // Slam position of selected blob
    bool processSBCentered;                 // Is selected blob centered

    // Delayed hole investigation
    DelayedHolePtr delayedHole;

    // Camera Inspect point
    Point3D camInspectPoint;                // In robot frame
    Point2D camInspectVector;
    std::string camInspectFrame;

    // Lost Hot Spot
    Point3D lostHSPointSlam;                // Slam position of the lost point

    bool blobCentreTimeout;                 // Centering of blobs has timed out
    Point3D victimPosRobot;                      // The victim position
};
typedef crosbot::Handle<VIDData> VIDDataPtr;

} // namespace casualty {

#endif /* VICTIMID_DATA_HPP_ */
