/*
 * File         : holeDetection.h
 * Author       : Timothy Wiley
 * Version      :
 * Created      : 21/04/2010
 * Description  : Finds holes/irregularities within the walls of the arena
 * Copyright    :
 */

#ifndef HOLE_DETECTOR_H_
#define HOLE_DETECTOR_H_

#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <tf/transform_listener.h>

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

#include "geometry.h"
#include "hole_detector/HoleMsg.h"
#include "hole_detector/HoleDetectorService.h"

#include <map>
#include <utility>
#include <sstream>

#define ELEMENT_HOLE_DETECTION          "HoleDetection"
#define NO_LASER_POINTS                 "no_laser_points"
#define REQUEST_POINT_CLOUD             "request_point_cloud"
#define POINTCLOUD_HOLEDETECTION        "pointcloud_holedetection"

using namespace crosbot;
using std::deque;
using std::pair;
using std::vector;

namespace casualty {

// Set edge Enum
enum EdgeType {
    ET_NOT_EDGE,
    ET_EDGE,
    ET_HOLE_POS,
    ET_HOLE_NEG,
    ET_VERTICAL_EDGE
};

// Polar Points that store data about them being edges
class EPolarPoint3D : public PolarPoint3D {
public:
    // Type of edge
    EdgeType edgeType;

    // Difference which caused this edge to be marked
    double diff;

    EPolarPoint3D() {};
    EPolarPoint3D(const EPolarPoint3D& p);
    EPolarPoint3D(PolarPoint3D& p);
    EPolarPoint3D(Point3D& p);
    ~EPolarPoint3D();
};

/*
 * Denotes a Hole, with a centre, negative and positive edge plus a perpendicular vector
 */
class Hole {
public:
    int id;             // Unique ID for this hole
    Point3D p;          // Centre of this hole
    Point3D neg;        // Average of negative side
    Point3D pos;        // Average of positive side
    Point2D perpend;    // Norm Vector
    std::string frame;  // Frame of the hole

    Hole(int id) :
        id(id)
    {}
    Hole(int id, Point3D p, Point3D neg, Point3D pos) :
        id(id), p(p), neg(neg), pos(pos), perpend()
    {};
    Hole(const Hole& other) :
        id(other.id), p(other.p),
        neg(other.neg), pos(other.pos), perpend(other.perpend),
        frame(other.frame)
    {}
    Hole(hole_detector::HoleMsg& msg) :
        id(msg.id),
        p(msg.p),
        neg(msg.neg),
        pos(msg.pos),
        perpend(msg.perpend.x, msg.perpend.y),
        frame(msg.frame)
    {}

    /**
     * Sets the norm vector of this hole
     */
    void setNorm();

    inline hole_detector::HoleMsg toROS() {
    	hole_detector::HoleMsg msg;
    	msg.id = id;
    	msg.p = p.toROS();
    	msg.neg = neg.toROS();
    	msg.pos = pos.toROS();
    	msg.perpend.x = perpend.x;
    	msg.perpend.y = perpend.y;
    	msg.perpend.z = 0;
    	msg.frame = frame;
    	return msg;
    }
};

/**
 * Extended version of a hole for internal processing
 */
class HoleExt : public Hole, public HandledObject {
public:
    vector<EPolarPoint3D*>  polars;     // Polars registered with this Hole
    pair<Point3D, int>      sumNegs;    // Sum and number of negative edges
    pair<Point3D, int>      sumPoss;    // Sum and number of positive edges
    double                  maxZ;       // Maximum z-value of this hole
    double                  minZ;       // Minimum z-value of this hole
    int                     clusterId;  // Id of cluster this hole is in

    HoleExt(int id) :
        Hole(id)
    {};
    HoleExt(int id, Point3D p, Point3D neg, Point3D pos) :
        Hole(id, p, neg, pos)
    {};
};
typedef Handle<HoleExt> HoleExtPtr;

// An extension of PointClouds that contain a list of polar points matching the coloured points
//    the polar points are always in the frame of reference of the sensor
//    Also has the 'theta' (elevation) angle of the point cloud
class EPointCloud : public PointCloud {
public:
    vector<EPolarPoint3D> polars;
    double theta;

    // Reference frames
    std::string frameClouds;        // Frame of PointCloud.clouds()
    std::string framePolars;        // Frame of PointCloud.points()

    // Create a new point cloud but does not copy any vector of points
    EPointCloud() {};
    ~EPointCloud();
};
typedef Handle<EPointCloud> EPointCloudPtr;

/**
 * Holes have a NEGATIVE EDGE followed by a POSITIVE EDGE
 */
class HoleDetector : public nodelet::Nodelet {
public:
    HoleDetector() {};
    ~HoleDetector() {};

    void configure(ConfigElementPtr config);
    void executeOnce();

protected:
    double consecutiveAngle;            // Maximum allowable angle for two edge points to be consecutive
    double distanceMaxThreshold;        // Threshold of how close points needs to be
    double distanceMinThreshold;        // Threshold of how close points needs to be
    std::string frameRobot;				// Robot frame
    std::string frameIcp;				// World/PosTrack frame
    double frequency;                   // Frequency to run hole detection
    double maxClusterDistance;          // In clustering, max distance on which to merge clusters
    double maxEdgeThreshold;            // Maximum Distance between points denoting an edge
    double maxHoleHeight;               // Maximum allowable height of a hole
    double maxHoleWidth;                // Maximum allowable width of the hole
    double maxHoleZ;                    // Maximum Z-Value a hole can be at
    double minEdgeThreshold;            // Minimum Distance between points denoting an edge
    double minHoleHeight;               // Minimum allowable height of a hole
    double minHoleWidth;                // Minimum allowable width of the hole
    double minHoleZ;                    // Minimum Z-Value a hole can be at
    double neighbourhoodDist;           // Maximum allowable distance between two points in the window
    std::string serviceInScansName;		// Identifier for postrack service
    std::string pubOutHoleCloudName;    // Hole Cloud Publisher name
    std::string pubOutHolesName;        // Hole Publisher name
    bool sendAllEdges;                  // Send all or only pos/neg edges
    bool sendPointCloud;                // Add coloured point clouds to the robot
    int  windowSize;                    // Window of distance between two points to consider as holes

    //int noThetaBins;
    //int noPhiBins;
    //double phiBinAngle;
    //double thetaBinAngle;

    // ROS elements
	ros::ServiceClient postrackClient;
	ros::ServiceServer holesService;
	ros::Publisher holeCloudPub;
	ros::Timer timerCallback;
	tf::TransformListener tfListener;

    // Point Cloud data
    deque<PointCloudPtr> pcs;              // Point clouds loaded from laser
    ros::Time pcsTimestamp;           // Timestamp of loaded point cloud
    vector<EPointCloudPtr> polarClouds;    // Polar Point clouds with edges

    // Saves data
    Mutex saveMutex;
    vector<Hole> savedHoles;

    // ROS virtual methods
    virtual void onInit();
    void callbackTimer(const ros::TimerEvent& tEvent);
    bool callbackHolesService(hole_detector::HoleDetectorServiceRequest& request,
    						  hole_detector::HoleDetectorServiceResponse& response);

    // Transform a given pose
    Pose transformPose(std::string fromFrame, std::string toFrame, ros::Time timestamp, Pose pose);

    // Edge Detection Algorithm(s)
    // Use Robert's Cross just on single laser scans
    void egdeDetectionRCSortedLists(vector<EPointCloudPtr>& polarClouds, double neighbourhood);
    // Edge detection on points in the vertical plane
    void verticalEdgeEdgeDetection(vector<EPointCloudPtr>& polarClouds, double neighbourhood);

    // General helper functions
    // Converts the theta angle into the theta bin no
    inline int thetaToBinNo(double angle);
    // Converts the phi angle into the theta bin no
    inline int phiToBinNo(double angle);
    // Make the ColouredPoint a edge/non-edge point
    Colour getEdgePointColour(Point3D& p, EdgeType edgeType);
    // Loads just the point cloud as a deque
    void loadDequePointCloud(deque<PointCloudPtr>& pcs);
    // Convert & Filter each laser scan into polar coordinates and keep each scan separate
    void toPolarAndFilter(deque<PointCloudPtr>& points, vector<EPointCloudPtr>& polarClouds);
    // Locates hole end points within the vectors of marked edges - updates points that are considered holes
    void locateHoleEnds(vector<EPointCloudPtr>& polarClouds);
    // Constructs the holes (and hole data
    void constructHoles(vector<EPointCloudPtr>& polarClouds, vector<HoleExtPtr>& holes);
    // Filters contructed holes based on various properties
    void filterHoles(vector<HoleExtPtr>& holes, vector<HoleExtPtr>& filtered);
    // Clusters holes to extract the centre of the holes
    void clusterHoles(vector<HoleExtPtr>& holes, vector<HoleExtPtr>& clusters);

    // Save holes
    void saveHoles(vector<HoleExtPtr>& holes);
    // Prepare a pointcloud for export to the robot
    PointCloudPtr preparePCforRobot(vector<EPointCloudPtr>& polarClouds, vector<HoleExtPtr>& holes);
    // Publish the coloured holes point cloud for graphical view
    void publishHolesPC(vector<EPointCloudPtr>& polarClouds, vector<HoleExtPtr>& holes);

    // Send Edges for point cloud split
    //void sendEdges(vector<EPointCloudPtr>& polarClouds);
    // Construct polar clouds from data buffer
    //void recieveEdges(DataBuffer buffer);

    // Bin the polar points into given unit size
    //void binPolarPoints(vector<PolarColouredPoint>& points, Bins& bins);
    // Cleans all the elements from the bins
    //void cleanBins(Bins& bins);

};

/* SEND/RECV Modes
#define POINTCLOUD_HOLEDETECTION_SEND   "pointcloud_holedetection_send"
#define POINTCLOUD_HOLEDETECTION_RECV   "pointcloud_holedetection_recv"
// Mode of hole detection for send/recieve/all
enum HDMode {
    HDM_ALL     = 0,
    HDM_SEND    = 1,
    HDM_RECV    = 2,
};
*/

/*
// A bin is a vector of PolarPoint3D's with some additional information
class Bin {
public:
    Bin();
    ~Bin();

    // Get average Distance
    double getAverageDist();
    // Get total Distance
    double getTotalDist();
    // Get if the bin is an edge bin
    bool isEdgeBin();

    // Access operators
    PolarColouredPoint operator[](int i);

    // Add polar point
    void add(PolarColouredPoint polar);
    // Number of polar points
    int size();
    // Remove all elements
    void clear();
    // Set it bin is an edge bin
    void setEdgeBin(bool edgeBin);

private:
    double averageDist;
    double totalDist;
    bool edgeBin;
    vector<PolarColouredPoint> polars;
};

// The bins are a 2 dimensional map of bins
typedef map<int, map<int, Bin* > > Bins;
*/

} // namespace casualty

#endif /* HOLE_DETECTOR_H_ */
