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

#include <crosbot/data.hpp>

namespace casualty {

class HoleDetector : public nodelet::Nodelet {
	ros::Subscriber imageSub, cloudSub;
	ros::Publisher cloudPub;

public:
	HoleDetector() {}

    void callbackImage(const sensor_msgs::ImageConstPtr& latestImage) {
    	printf("HoleDetector:: image received.\n");

    	printf("HoleDetector:: image size(%u * %u)\n", latestImage->width, latestImage->height);
    }

    void callbackCloud(const sensor_msgs::PointCloud2Ptr& latestCloud) {
    	printf("HoleDetector:: cloud received.\n");

    	crosbot::PointCloud cloud(*latestCloud);

    	std::vector< double > angles;
    	angles.resize(cloud.cloud.size());

    	uint32_t validAngles = 0, validEdges = 0;
    	uint32_t width = latestCloud->width, height = latestCloud->height;
    	for (uint32_t y = 0; y < height; ++y) {
    		for (uint32_t x = 0; x < width; ++x) {
				uint32_t idx = y * width + x;
				if (x < 10 || x >= width - 11) {
					angles[idx] = 0;
					continue;
				}

				if (cloud.cloud[idx].hasNAN() || cloud.cloud[idx-1].hasNAN() || cloud.cloud[idx+1].hasNAN()) {
					angles[idx] = 0;
					continue;
				}

				crosbot::Point v1 = cloud.cloud[idx] - cloud.cloud[idx-1],
						v2 = cloud.cloud[idx+1] - cloud.cloud[idx],
						v3 = cloud.cloud[idx] - cloud.cloud[idx-1],
						v4 = cloud.cloud[idx+10] - cloud.cloud[idx];

				if (v1.hasNAN() || v2.hasNAN() || v3.hasNAN() || v4.hasNAN()) {
					angles[idx] = 0;
					continue;
				}

				angles[idx] = (acos(v1.normalise().dot(v2.normalise())) +
						acos(v3.normalise().dot(v4.normalise()))) / 2;
				validAngles++;
			}
    	}

    	printf("HoleDetector:: Have %u valid angles from %zu.\n", validAngles, width*height);

    	if (cloud.colours.size() == 0) {
    		cloud.colours.resize(cloud.cloud.size());
    	}
    	crosbot::Colour leadEdge(255,0,0), flat(255,255,255);
    	for (uint32_t idx = 0; idx < cloud.cloud.size(); ++idx) {
    		if (fabs(angles[idx]) >= 1) {
    			cloud.colours[idx] = leadEdge;
    			validEdges++;
    		} else {
    			cloud.colours[idx] = flat;
    		}
    	}
    	printf("HoleDetector:: Have %u valid edges from %zu.\n", validEdges, validAngles);

    	if (cloudPub.getNumSubscribers() > 0)
    		cloudPub.publish(cloud.tosROS1());
    }

	virtual void onInit() {
		imageSub = getNodeHandle().subscribe("image", 1, &HoleDetector::callbackImage, this);
		cloudSub = getNodeHandle().subscribe("cloud", 1, &HoleDetector::callbackCloud, this);

		cloudPub = getNodeHandle().advertise<sensor_msgs::PointCloud>("out_cloud", 1);
	}
};

} // namespace casualty

PLUGINLIB_DECLARE_CLASS(hole_detector, hole_detector, casualty::HoleDetector, nodelet::Nodelet)
