/*
 * File         : thermal_blobber.h
 * Author       : Timothy Wiley
 * Version      :
 * Created      : 11/02/2010
 * Description  : Detects hot spots (blobs) on thermal images
 * Copyright    :
 */

#ifndef THERMAL_BLOBBER_H_
#define THERMAL_BLOBBER_H_

#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>

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

#include "blob.h"
#include "thresholdCal.h"
#include "cameraCorrection.h"
#include "thermal_blobber/BlobsService.h"

#include <vector>

using namespace crosbot;

namespace casualty {

#define ELEMENT_BLOBBER "Blobber"

class ThermalBlobber : public nodelet::Nodelet {
public:
	ThermalBlobber();

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

    // Blob information
    // Assigns the blobs and related thermal image from the latest data
	//virtual void getBlobs(std::vector<Blob>& blobs);

    /**
     * Get the thermal camera instance being used by the blobber
     * @return Handle to thermal camera or NULL if camera is not setup
     */
	//virtual Camera getThermalCam();

    /**
     * Get the blob current threshold
     * @return Current blob threshold
     */
	//virtual unsigned short getThreshold();

protected:
    // Configuration Information
	bool            autoThreshold;
	std::string     frameThermal;           // Reference frame of thermal camera
	double          frequency;              // Frequency to update/reprocess blobber
	int             initialThreshold;
	double          learningRate;
	int             minimumArea;            // Minimum area that a blob must be
	unsigned short  minimumThreshold;       // Minimum value a threshold can be set to
	int             minimumWidth;           // Minimum with of the blob
	bool            invert;                 // Invert the image
	bool            savedData;              // True if saved data is available

    // To correct for the thermal camera that has padding
	bool removePadding;
	unsigned int removePaddingTop;
	unsigned int removePaddingRight;
	unsigned int removePaddingBottom;
	unsigned int removePaddingLeft;

    // Auto Thresholder
	ThresholdCalPtr thresholdCal;

    // ROS elements
	image_transport::Subscriber thermalSub, distanceSub;
	//ros::Publisher blobsPub;
	ros::ServiceServer blobsService;
	image_transport::Publisher blobsImagePub, thresholdImagePub;
	ros::Timer timerCallback;

    // Images from callbacks
	sensor_msgs::ImageConstPtr nextThermalImage;
	sensor_msgs::ImageConstPtr nextDistanceImage;

    // Images for processing
	ImagePtr thermalImage;
	ImagePtr distanceImage;
	ros::Time thermalTimestamp;

    CameraCorrectionPtr cameraCorrection;

    // Concurrency controls
	Mutex loadMutex;
	Mutex saveMutex;

	// Tells us if the blobber is fully initialised
	bool initialised;

    // Blob for detecting
	unsigned short threshold;		// Current threshold
	std::vector<BlobPtr> blobs;		// Blobs list
	std::vector<int> equiv;			// Set of equivalent ids

	// Saved data
	std::vector<BlobPtr> savedBlobs;
	ImagePtr savedEncodedImage;
	ImagePtr savedDistanceImage;
	ros::Time savedThermalTimestamp;
	int savedThreshold;

    // ROS callbacks
	void callbackTimer(const ros::TimerEvent& tEvent);
	void callbackThermalImage(const sensor_msgs::ImageConstPtr& latestImage);
	void callbackDistanceImage(const sensor_msgs::ImageConstPtr& latestImage);
	bool callbackBlobsService(thermal_blobber::BlobsServiceRequest& request,
						      thermal_blobber::BlobsServiceResponse& response);
	virtual void onInit();

    // Methods
    // Load the new images
	void loadNewImages();
    // Removes the padding from the thermal image
	ImagePtr removePaddingFromImage(ImagePtr image);
    // Returns the percentage of the thermal image covered if the given threshold is used
	double blobCoverage(std::vector<BlobPtr>& blobs, ImagePtr processImage);
    // Returns the highest grey level in the current set of images
	unsigned short getHighestGreyLevel(ImagePtr processImage);
    // Sets the threshold initially
	void setInitialThreshold();
    // Simple Thresholder
	void simpleThreshold(ImagePtr image, unsigned short threshold);
    // Locate blobs within image
	void findBlobs(ImagePtr image, std::vector<BlobPtr>& blobs, std::vector<int>& equiv, int blobIndex[]);
    // Create blobs given an array of the blob locations in the image
	void extractBlobs(int blobIndex[], int width, int height, std::vector<BlobPtr>& blobs, std::vector<int>& equiv);
    // Remove any undersized blobs
	void trimBlobs(int minArea, int minWidth, std::vector<BlobPtr>& blobs);
    // Create the encoded thermal image to map image points to blobs
	void encodedBlobsImage(ImagePtr image, vector<BlobPtr>& blobs, int blobIndex[]);
	// Draw the borders of the blobs on the processed image
	void drawBlobBorders(ImagePtr image, std::vector<BlobPtr>& blobs);
    // Save the blobs & threshold image for external classes to use
	void saveData(std::vector<BlobPtr>& blobs, ImagePtr thresholdImg, ImagePtr encodedImage,
					 ImagePtr distanceImage);
    // Update the threshold
	void updateThreshold(ImagePtr processImage);

    /* Helper methods to be inlined */
    // Gets the blob no which that the supplied no. is equivalent to.
	inline int equivalentBlob(int blobNo, std::vector<int> equiv);
    // Check if the blob meets the minimum requirements
	inline bool blobSizeOk(BlobPtr blob, int minArea, int minWidth);
    // Check if is contained within another blob
	bool containedInside(BlobPtr toTest, std::vector<BlobPtr>& blobs);

    /**
     * Invert the given image, reversing black and white colours. This lets
     * the blobber detect black spots instead
     * @param image Image to invert
     */
	void invertImage(ImagePtr image);

	// Calculate the distances for all the blobs
	void calculateBlobPoints(std::vector<BlobPtr>& blobs, int blobIndex[]);
	// Calculate the average distance for a given blob
	//void calculateAverageDistanceKinect(BlobPtr blob);
};

} // namespace casualty

#endif /* THERMAL_BLOBBER_H_ */
