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

#include "hole_detector/holeDetector.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 <crosbot/PointCloudMsg.h>
#include <crosbot_ogmbicp/GetRecentScans.h>

#include <algorithm>
#include <cmath>
#include <deque>
#include <limits>

#define DEFAULT_BIN_ANGLE   		1.0
#define DEFAULT_MAXWAIT_POSTRACK	2.0
#define DEFAULT_MAXWAIT_TRANSFORM	2.0
#define LOG_START           		"HoleDetection ::"
#define RC_NO_INDEX         		-1

#define SEC2USEC(n) (n * 1000000)

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

// Declare the class for ROS
PLUGINLIB_DECLARE_CLASS(hole_detector, hole_detector, casualty::HoleDetector, nodelet::Nodelet)

namespace casualty {

/* CLUSTER DISTANCE ***********************************************************/
typedef struct ClusterDist {
    int indexA;
    int indexB;
    double dist;

    ClusterDist() : indexA(0), indexB(0), dist(0) {};
    ClusterDist(int idA, int idB, double dist) : indexA(idA), indexB(idB), dist(dist) {};
} ClusterDist;

/* Custom sorting functions for edgeDetectionRCSortedList *********************/
bool sortTheta(PolarColouredPoint a, PolarColouredPoint b) {
    return a.theta < b.theta;
}
bool sortPhi(PolarColouredPoint a, PolarColouredPoint b) {
    return a.phi < b.phi;
}
bool sortPCTheta(EPointCloudPtr a, EPointCloudPtr b) {
    return a->theta < b->theta;
}
bool sortClusterDist(ClusterDist a, ClusterDist b) {
    return a.dist < b.dist;
}

/* EPolarPoint3D **************************************************************/
EPolarPoint3D::EPolarPoint3D(const EPolarPoint3D& p) :
    PolarPoint3D(p)
{
    edgeType = p.edgeType;
    diff = p.diff;
}

EPolarPoint3D::EPolarPoint3D(PolarPoint3D& p) :
    PolarPoint3D(p)
{
    edgeType = ET_NOT_EDGE;
    diff = 0;
}

EPolarPoint3D::EPolarPoint3D(Point3D& p) :
    PolarPoint3D(p)
{
    edgeType = ET_NOT_EDGE;
    diff = 0;
}

EPolarPoint3D::~EPolarPoint3D()
{}

EPointCloud::~EPointCloud() {
}

/* Hole non-inline ************************************************************/
void Hole::setNorm() {
    Point3D diffVec = neg - pos;
    Point3D perpend3d = diffVec.cross(Point3D(0, 0, 1));
    perpend3d = perpend3d.normalise();
    perpend = Point2D(perpend3d.x, perpend3d.y);

    // Set Norm
    //LOG("****** Hole :: Setting Normal Vector: %s\n", perpend.toString().c_str());
}

/* HoleDetection *************************************************************/
void HoleDetector::callbackTimer(const ros::TimerEvent& tEvent) {
	//LOG("%s callbackTimer \n", LOG_START);
	executeOnce();
}

bool HoleDetector::callbackHolesService(hole_detector::HoleDetectorServiceRequest& request,
    						  hole_detector::HoleDetectorServiceResponse& response) {
	Lock lock(saveMutex);

	// Set holes
	for(vector<Hole>::iterator it = savedHoles.begin(); it != savedHoles.end(); ++it) {
	    //LOG("%s Callback hole: %s\n", LOG_START, it->p.toString().c_str());
		response.holes.push_back(it->toROS());
	}

	// Set point cloud timestamp
	response.pcTimestamp = pcsTimestamp;

	return true;
}

Pose HoleDetector::transformPose(string fromFrame, string toFrame, ros::Time timestamp, Pose pose) {
    //LOG("%s transformPose\n", LOG_START);
    std::string error;
    bool transformLoaded = tfListener.waitForTransform(toFrame, fromFrame, timestamp,
            ros::Duration(DEFAULT_MAXWAIT_TRANSFORM), ros::Duration(0.01), &error);
    if (!transformLoaded) {
        //LOG("%s \t\t Could not load transform from %s to %s\n", LOG_START, fromFrame.c_str(), toFrame.c_str());
        //LOG("%s \t\t Error: %s\n", LOG_START, error.c_str());
        return pose;
    }
    //LOG("%s \t got transform\n", LOG_START);

    // Do transform
    geometry_msgs::PoseStamped fromPose;
    fromPose.header.frame_id = fromFrame;
    fromPose.header.stamp = timestamp;
    fromPose.pose = pose.toROS();
    geometry_msgs::PoseStamped toPose;
    tfListener.transformPose(toFrame, fromPose, toPose);
    return Pose(toPose.pose);
}

void HoleDetector::onInit() {
	LOG("%s init\n", LOG_START);

	// Setup variables
    pcs.clear();
    savedHoles.clear();

	// Load configuration
	ros::NodeHandle nhConfig("/hole_detector");
	ConfigElementPtr config = new ROSConfigElement(nhConfig);
	configure(config);

	// Setup callbacks
	ros::NodeHandle& nh = getNodeHandle();
	timerCallback = nh.createTimer(ros::Duration(frequency), &HoleDetector::callbackTimer, this);
	holeCloudPub = nh.advertise<sensor_msgs::PointCloud>(pubOutHoleCloudName, 1);
	holesService = nh.advertiseService(pubOutHolesName, &HoleDetector::callbackHolesService, this);
	postrackClient = nh.serviceClient<crosbot_ogmbicp::GetRecentScans>(serviceInScansName, true);	// true = persistent connection
}

void HoleDetector::configure(ConfigElementPtr config) {
    LOG("%s configure\n", LOG_START);

    // Load other config
    consecutiveAngle        = config->getParamAsDouble("consecutiveAngle", (270.0 / 1081.0) * 10);
    distanceMaxThreshold    = config->getParamAsDouble("distanceMaxThreshold", 10.0);
    distanceMinThreshold    = config->getParamAsDouble("distanceMinThreshold", 0.2);
    frameRobot				= config->getParam("frameRobot", "/base_footprint");
    frameIcp				= config->getParam("frameIcp", "/icp");
    frequency               = config->getParamAsDouble("frequency", 2.0);
    maxClusterDistance      = config->getParamAsDouble("maxClusterDistance", 0.25);
    maxEdgeThreshold        = config->getParamAsDouble("maxEdgeThreshold", 1.0);
    maxHoleHeight           = config->getParamAsDouble("maxHoleHeight", 2.0);
    maxHoleWidth            = config->getParamAsDouble("maxHoleWidth", 1.0);
    maxHoleZ                = config->getParamAsDouble("maxHoleZ", 2.0);
    minEdgeThreshold        = config->getParamAsDouble("minEdgeThreshold", 0.0);
    minHoleHeight           = config->getParamAsDouble("minHoleHeight", 0.0);
    minHoleWidth            = config->getParamAsDouble("minHoleWidth", 0.2);
    minHoleZ                = config->getParamAsDouble("minHoleZ", 0.0);
    neighbourhoodDist       = config->getParamAsDouble("neighbourhoodDist", 1.0);
    serviceInScansName		= config->getParam("serviceInScans", "in_scans");
    pubOutHoleCloudName     = config->getParam("pubOutHoleCloud", "out_hole_cloud");
    pubOutHolesName         = config->getParam("pubOutHoles", "out_holes");
    sendAllEdges            = config->getParamAsBool("sendAllEdges", true);
    sendPointCloud          = config->getParamAsBool("sendPointCloud", false);
    windowSize              = config->getParamAsInt("windowSize", 5);

    LOG("%s windowSize: %d\n", LOG_START, windowSize);

    /*
    thetaBinAngle = phiBinAngle = config->getAttributeAsDouble("binAngle", DEFAULT_BIN_ANGLE);
    if (config->hasAttribute("thetaBinAngle")) {
        thetaBinAngle = config->getAttributeAsDouble("thetaBinAngle", DEFAULT_BIN_ANGLE);
    }
    if (config->hasAttribute("phiBinAngle")) {
        phiBinAngle = config->getAttributeAsDouble("phiBinAngle", DEFAULT_BIN_ANGLE);
    }
    // Load number of theta/phi bins
    noThetaBins = ceil(360 / thetaBinAngle);
    noPhiBins = ceil(360 / phiBinAngle);
    */

    LOG("%s Frequency: %lf\n", LOG_START, frequency);
}

void HoleDetector::executeOnce() {
    //LOG("%s Executing.\n", LOG_START);

    // Common variables
    vector<HoleExtPtr> holes;
    vector<HoleExtPtr> filtered;
    vector<HoleExtPtr> clusters;
    pcs.clear();
    pcsTimestamp = ros::Time::now();
    polarClouds.clear();

	// Load the next laser points
	//LOG("%s Load points\n", LOG_START);
	loadDequePointCloud(pcs);
	//LOG("%s \t\t Num Point Clouds: %ld\n", LOG_START, pcs.size());

	// Convert to Polar, Filter
	//LOG("%s \t Polar and Filter\n", LOG_START);
	toPolarAndFilter(pcs, polarClouds);

	// Run edge detection
	//LOG("%s \t Edge Detection\n", LOG_START);
	egdeDetectionRCSortedLists(polarClouds, neighbourhoodDist);

	// Run Locate Holes End points
	//LOG("%s \t Locate Hole end-points. Num PolarClouds: %ld\n", LOG_START, polarClouds.size());
	locateHoleEnds(polarClouds);

	// Construct hole (centres)
	//LOG("%s \t Construct holes\n", LOG_START);
	constructHoles(polarClouds, holes);
	//LOG("%s \t\t Number original holes: %ld\n", LOG_START, holes.size());

	// Filter constructed holes
	//LOG("%s \t Filter holes\n", LOG_START);
	filterHoles(holes, filtered);
	//LOG("%s \t\t Number filtered holes: %ld\n", LOG_START, filtered.size());

	// Run Clustering
	//LOG("%s \t Cluster holes\n", LOG_START);
	clusterHoles(filtered, clusters);
	//LOG("%s \t\t Num Clusters: %ld\n", LOG_START, clusters.size());

	// Save the cluster centres
	saveHoles(clusters);

	// Locate top of panels
	//LOG("%s \t Locate arena top\n", LOG_START);
	//verticalEdgeEdgeDetection(polarClouds, neighbourhoodDist);

	// Send point cloud to the robot
	if (sendPointCloud) {
		//LOG("%s \t Send Point Cloud to Visualiser\n", LOG_START);
		publishHolesPC(polarClouds, clusters);
	}

	// Clean up
	pcs.clear();
	polarClouds.clear();
	//LOG("%s \t Done executeOnce\n", LOG_START);
}

void HoleDetector::loadDequePointCloud(deque<PointCloudPtr>& pcs) {
    // Wait for postrack service
	bool foundService = ros::service::waitForService(serviceInScansName, ros::Duration(DEFAULT_MAXWAIT_POSTRACK));
	if (!foundService) {
		LOG("%s \t\t Failed to find Postrack: %s\n", LOG_START, serviceInScansName.c_str());
		return;
	}
	//LOG("%s \t\t Found Postrack\n", LOG_START);

	// Service callback
	crosbot_ogmbicp::GetRecentScansRequest request;
	crosbot_ogmbicp::GetRecentScansResponse response;
	bool callbackSuccess = postrackClient.call(request, response);
	if (!callbackSuccess) {
		//LOG("%s \t\t callbackFailed\n", LOG_START);
		return;
	}

	// Set timestamp
	pcsTimestamp = response.scans.header.stamp; // TODO Check to add back
	//pcsTimestamp = ros::Time::now();

	// Processing data
	int nScans = response.scans.height;
	int nPoints = response.scans.width;
	int floatSize = sizeof(float);
	int sizePoint = floatSize * 3;
	int sizeScan = sizePoint * nPoints;
	int index = 0;
	int indexInc = floatSize / sizeof(uint8_t);

	// Load points
	for (int scan = 0; scan != nScans; ++scan) {
		index = scan * sizeScan;

		PointCloudPtr pc = new crosbot::PointCloud();
		for (int point = 0; point != nPoints; ++point) {
			// Extract point
			Point3D p;
			p.x = *((float*) &(response.scans.data[index]));
			index += indexInc;
			p.y = *((float*) &(response.scans.data[index]));
			index += indexInc;
			p.z = *((float*) &(response.scans.data[index]));
			index += indexInc;

			// Save point
			pc->cloud.push_back(p);
		}

		// Save cloud
		pcs.push_back(pc);
	}

	//LOG("%s \t\t Timestamp %lf\n", LOG_START, pcsTimestamp.toSec());
}

void HoleDetector::toPolarAndFilter(deque<PointCloudPtr>& points, vector<EPointCloudPtr>& polarClouds) {
	// Get latest ICP pose of the robot
	Pose robotPose;
	//Pose robotIcpPose = transformPose(frameRobot, frameIcp, pcsTimestamp, robotPose);
	Pose robotIcpInverse = transformPose(frameIcp, frameRobot, pcsTimestamp, robotPose);
	//LOG("%s \t robotPos: %s\n", LOG_START, robotIcpPose.position.toString().c_str());

	//int pointsCount = 0;
	//for (int i = 0; i != points.size(); ++i) {
	//    pointsCount += points[i]->cloud.size();
	//}
	//LOG("%s \t points: %d\n", LOG_START, pointsCount);

	for(deque<PointCloudPtr>::iterator it = points.begin(); it != points.end(); ++it) {
    	PointCloudPtr icppc = *it;

    	PointCloudPtr rpc = new PointCloud(frameRobot, *icppc, robotIcpInverse);
        EPointCloudPtr mpc = new EPointCloud();
        mpc->frameClouds = frameIcp;
        mpc->framePolars = frameRobot;

        // Records the centre most point
        //int centreIndex = -1;
        //double centreAngleDiff = numeric_limits<double>::max();

        for(unsigned int i = 0; i < icppc->cloud.size(); ++i) {
            EPolarPoint3D robotPolar(rpc->cloud[i]);
            if (robotPolar.dist >= distanceMinThreshold &&
                robotPolar.dist <= distanceMaxThreshold) {
                // Translate point to make 0 deg directly behind laser
                //    normally behind laser is -90 deg.
                //    third quadrant is -180 to -90 in polar, and must be have an extra 360 added
                if (robotPolar.phi < -90) {
                    robotPolar.phi += 450;
                } else {
                    robotPolar.phi += 90;
                }

                //LOG("%s LaserPoint (world): %s\n", LOG_START, wpc->laserPoints[i].toString().c_str());
                mpc->cloud.push_back(icppc->cloud[i]);
                mpc->polars.push_back(robotPolar);
            }

            // Find the point with the most central phi (closest to 180 deg)
            //double diff = fabs(180 - robotPolar.phi);
            //if (diff < centreAngleDiff) {
            //    centreIndex = i;
            //    centreAngleDiff = diff;
            //}
        }

        // Set point cloud theta angle
        //if (centreIndex != -1) {
        //    PolarPoint3D centreEpp(tpc->laserPoints[centreIndex]);
        //    mpc->theta = centreEpp.theta;
        //}

        polarClouds.push_back(mpc);
    }
}

void HoleDetector::egdeDetectionRCSortedLists(vector<EPointCloudPtr>& polarClouds, double neighbourhood) {
    // Process each scan
    for(vector<EPointCloudPtr>::iterator it = polarClouds.begin(); it != polarClouds.end(); ++it) {
        EPointCloudPtr mpc = *it;

        // Iterate through each point and work out distance
        for(int i = windowSize; (i + windowSize) < (int) mpc->polars.size(); ++i) {
            // Get point and window
            EPolarPoint3D& prev = mpc->polars[i - windowSize];
            EPolarPoint3D& p = mpc->polars[i];
            EPolarPoint3D& next = mpc->polars[i + windowSize];
            //LOG("%s\tphi: %lf, dist: %lf, x:%lf, y:%lf, z:%lf\n", LOG_START, p.phi, p.dist, p.x, p.y, p.z);

            // Assume point is not an edge point
            p.edgeType = ET_NOT_EDGE;

            double prevDist = fabs(prev.phi - p.phi);
            double nextDist = fabs(next.phi - p.phi);
            double diff = prev.dist - next.dist;
            if (prevDist <= neighbourhood && nextDist <= neighbourhood) {
                //LOG("%s \t within neighbourhood\n", LOG_START);
                // Calculate difference in distance
                if (fabs(diff) >= minEdgeThreshold && fabs(diff) <= maxEdgeThreshold) {
                    // Mark point
                    //LOG("%sdiff: %lf\n", LOG_START, diff);
                    p.edgeType = ET_EDGE;
                    p.diff = diff;
                }
            }
        }
    }
}

void HoleDetector::locateHoleEnds(vector<EPointCloudPtr>& polarClouds) {
    for(vector<EPointCloudPtr>::iterator it = polarClouds.begin(); it != polarClouds.end(); ++it) {
        EPointCloudPtr mpc = *it;
        //bool foundStartHole = false;
        //double angleStartHole = 0;

        // Iterate through each point and work out distance
        unsigned int i = 0;
        while(i != mpc->polars.size()) {
            EPolarPoint3D& p = mpc->polars[i];

            if (p.edgeType == ET_EDGE) {
                //LOG("%s p[%d](%lf) - diff: %.3lf\n", LOG_START, i, p.phi, p.diff);

                // If not negative, then not start of a hole, move onto next i
                if (p.diff >= 0) {
                    //LOG("%s \t Not negative edge - go to next edge\n", LOG_START);
                    i++;
                    continue;
                }

                // Collect sequential negative edge
                unsigned int negStart = i;
                unsigned int negEnd = negStart;
                i++;
                while (i < mpc->polars.size() &&
                       mpc->polars[i].diff < 0) {
                    if ((mpc->polars[i].phi - mpc->polars[i-1].phi) > consecutiveAngle) {
                        // Start of new negative hole
                        //LOG("%s p[%d](%lf) - diff: %.3lf\n", LOG_START, i, mpc->polars[i].phi, mpc->polars[i].diff);
                        //LOG("%s \t Resetting negative start: diff %.3lf (max: %.3lf)\n", LOG_START, (mpc->polars[i].phi - mpc->polars[i-1].phi), consecutiveAngle);
                        negStart = i;
                    }
                    negEnd = i;
                    i++;
                }

                // Go to next positive edge
                while (i < mpc->polars.size() &&
                       mpc->polars[i].edgeType != ET_EDGE) {
                    i++;
                }

                // Ensure there are edges left
                if (i < mpc->polars.size()) {
                    // Collect positive edges
                    unsigned int posStart = i;
                    unsigned int posEnd = posStart;
                    i++;
                    while (i < mpc->polars.size() &&
                           //mpc->polars[i].diff > 0) {
                           mpc->polars[i].diff > 0 &&
                           (mpc->polars[i].phi - mpc->polars[i-1].phi) <= consecutiveAngle) {
                        posEnd = i;
                        i++;
                    }

                    // Mark positive and negative hole ends
                    for(unsigned int j = negStart; j <= negEnd; j++) {
                        mpc->polars[j].edgeType = ET_HOLE_NEG;
                    }
                    for(unsigned int j = posStart; j <= posEnd; j++) {
                        mpc->polars[j].edgeType = ET_HOLE_POS;
                    }
                }
            } else {
                // Not an edge, increment to next point
                i++;
            }
        }
    }
}

void HoleDetector::constructHoles(vector<EPointCloudPtr>& polarClouds, vector<HoleExtPtr>& holes) {
    int idCounter = 0;

    for(vector<EPointCloudPtr>::iterator it = polarClouds.begin(); it != polarClouds.end(); ++it) {
        EPointCloudPtr mpc = *it;
        for(unsigned int i = 0 ; i < mpc->polars.size(); ++i) {
            // Look for starting edge (negative)
            if (mpc->polars[i].edgeType == ET_HOLE_NEG) {
                // Extended hole to add values to at end
                HoleExtPtr hole = new HoleExt(idCounter);
                hole->frame = mpc->framePolars;
                hole->sumNegs = std::make_pair<Point3D, int>(Point3D(), 0);
                hole->sumPoss = std::make_pair<Point3D, int>(Point3D(), 0);
                hole->maxZ = std::numeric_limits<double>::min();
                hole->minZ = std::numeric_limits<double>::max();

                // Get negative hole counts
                Point3D negTally(0, 0, 0);
                int negCount = 0;
                int j = i;
                while (mpc->polars[j].edgeType == ET_HOLE_NEG) {
                    // User world coordinates for tally
                    hole->polars.push_back(&(mpc->polars[j]));
                    hole->sumNegs.first = hole->sumNegs.first + mpc->polars[j];
                    hole->sumNegs.second++;
                    hole->maxZ = std::max(hole->maxZ, mpc->polars[j].z);
                    hole->minZ = std::min(hole->minZ, mpc->polars[j].z);
                    negTally = negTally + mpc->polars[j];
                    negCount++;
                    j++;
                }

                // Move onto positive holes
                while (mpc->polars[j].edgeType != ET_HOLE_POS) {
                    j++;
                }

                // Add positive hole counts
                Point3D posTally(0, 0, 0);
                int posCount = 0;
                while (mpc->polars[j].edgeType == ET_HOLE_POS) {
                    hole->polars.push_back(&(mpc->polars[j]));
                    hole->sumPoss.first = hole->sumPoss.first + mpc->polars[j];
                    hole->sumPoss.second++;
                    hole->maxZ = std::max(hole->maxZ, mpc->polars[j].z);
                    hole->minZ = std::min(hole->minZ, mpc->polars[j].z);
                    posTally = posTally + mpc->polars[j];
                    posCount++;
                    j++;
                }

                // Construct hole
                hole->neg   = negTally / (double) negCount;
                hole->pos   = posTally / (double) posCount;
                hole->p     = (hole->neg + hole->pos) / 2.0;
                holes.push_back(hole);
                idCounter++;

                // Set i
                i = j;
                i--;
            }
        }
    }
}

void HoleDetector::filterHoles(vector<HoleExtPtr>& holes, vector<HoleExtPtr>& filtered) {
    for(unsigned int i = 0; i != holes.size(); i++) {
        HoleExtPtr hole = holes[i];
        bool keepHole = true;

        double distNegToPos = hole->neg.distanceTo(hole->pos);
        double height = hole->maxZ - hole->minZ;
        if (distNegToPos > maxHoleWidth) {
            keepHole = false;
        } else if (distNegToPos < minHoleWidth) {
            keepHole = false;
        } else if (hole->p.z > maxHoleZ) {
            keepHole = false;
        } else if (hole->p.z < minHoleZ) {
            keepHole = false;
        } else if (height > maxHoleHeight) {
            keepHole = false;
        } else if (height < minHoleHeight) {
            keepHole = false;
        }

        if (keepHole) {
            //LOG("%s \t\t Keeping Hole: %s (dist: %.3lf)\n", LOG_START, hole->p.toString().c_str(), distNegToPos);
            filtered.push_back(hole);
        } else {
            //LOG("%s \t\t Discarding Hole: %s (dist n->p: %.3lf) (height: %.3lf)\n", LOG_START, hole->p.toString().c_str(), distNegToPos, height);
            for (unsigned int j = 0; j != hole->polars.size(); j++) {
                hole->polars[j]->edgeType = ET_NOT_EDGE;
            }
        }
    }
}

void HoleDetector::clusterHoles(vector<HoleExtPtr>& holes, vector<HoleExtPtr>& clusters) {
    // Setup structure with holes within the same cluster
    map<int, vector<HoleExtPtr> > clusterMap;
    int maxClusterId = -1;
    for (unsigned int i = 0; i < holes.size(); i++) {
        maxClusterId++;
        holes[i]->clusterId = maxClusterId;
        clusterMap[maxClusterId] = vector<HoleExtPtr>(1, holes[i]);
        //LOG("%s \t\t Set cluster id: %d\n", LOG_START, maxClusterId);
    }

    // Construct pairs of holes to merge
    deque<ClusterDist> toMerge;
    for (unsigned int i = 0; i < holes.size(); i++) {
        for (unsigned int j = i+1; j < holes.size(); j++) {
            double dist = holes[i]->p.distanceTo(holes[j]->p);
            if (dist < maxClusterDistance) {
                toMerge.push_back(ClusterDist(i, j, dist));
            }
        }
    }
    //LOG("%s \t\t Number of hole pairs to merge: %d\n", LOG_START, toMerge.size());

    // For each pair - merge
    for (unsigned int i = 0; i != toMerge.size(); i++) {
        ClusterDist& merge = toMerge[i];

        // If already in the same cluster, go to next pair
        int clusterIdA = holes[merge.indexA]->clusterId;
        int clusterIdB = holes[merge.indexB]->clusterId;
        //LOG("%s \t\t Merging hole %d(%d) \t with %d(%d)\n", LOG_START, merge.indexA, clusterIdA, merge.indexB, clusterIdB);
        if (clusterIdA == clusterIdB) {
            continue;
        }

        // Otherwise, create new cluster from previous two cluster
        maxClusterId++;
        //LOG("%s \t\t\t New cluster id: %d\n", LOG_START, maxClusterId);
        clusterMap[maxClusterId] = vector<HoleExtPtr>();
        vector<HoleExtPtr>& newCluster = clusterMap[maxClusterId];

        newCluster.insert(newCluster.end(), clusterMap[clusterIdA].begin(), clusterMap[clusterIdA].end());
        newCluster.insert(newCluster.end(), clusterMap[clusterIdB].begin(), clusterMap[clusterIdB].end());

        // Remove old clusters
        clusterMap.erase(clusterIdA);
        clusterMap.erase(clusterIdB);

        // Set new cluster id on each element in new cluster
        for (unsigned int j = 0; j != newCluster.size(); j++) {
            newCluster[j]->clusterId = maxClusterId;
        }
        //LOG("%s \t\t\t Done setting id's\n", LOG_START);
    }

    // Create actual clusters
    //LOG("%s \t\t Creating actual clusters\n", LOG_START);
    for (map<int, vector<HoleExtPtr> >::iterator it = clusterMap.begin(); it != clusterMap.end(); ++it) {
        int id = it->first;
        vector<HoleExtPtr>& cluster = it->second;

        // Get tallies of negative and positive sides
        double tallyNegs = 0;
        double tallyPoss = 0;
        Point3D pointNegs;
        Point3D pointPoss;

        for (unsigned int j = 0; j != cluster.size(); j++) {
            pointNegs       = pointNegs + cluster[j]->sumNegs.first;
            tallyNegs      += cluster[j]->sumNegs.second;
            pointPoss       = pointPoss + cluster[j]->sumPoss.first;
            tallyPoss      += cluster[j]->sumPoss.second;
        }

        // Create and add hole to clusters
        HoleExtPtr hole = new HoleExt(id);
        hole->neg   = pointNegs / tallyNegs;
        hole->pos   = pointPoss / tallyPoss;
        hole->p     = (hole->neg + hole->pos) / 2.0;
        //LOG("%s \t\t\t Added cluster: p:%s \t neg:%s \t pos:%s\n", LOG_START,
        //        hole->p.toString().c_str(), hole->neg.toString().c_str(), hole->pos.toString().c_str());
        clusters.push_back(hole);
    }
}

void HoleDetector::saveHoles(vector<HoleExtPtr>& holes) {
    Lock lock(saveMutex);
    savedHoles.clear();
    for(unsigned int i = 0;  i != holes.size(); i++) {
        Hole toSave(*(holes[i]));
        toSave.frame = frameRobot;
        toSave.setNorm();
        //LOG("%s Saved hole: %s\n", LOG_START, toSave.p.toString().c_str());
        savedHoles.push_back(toSave);
    }
}

PointCloudPtr HoleDetector::preparePCforRobot(vector<EPointCloudPtr>& polarClouds, vector<HoleExtPtr>& holes) {
    PointCloudPtr toRobot = new PointCloud();

	toRobot->frameID = "/base_footprint";
    //LOG("%s \t\t Sending description: %s\n", LOG_START, toRobot->description.c_str());

    // Iterate through the scans
    for(vector<EPointCloudPtr>::iterator it = polarClouds.begin(); it != polarClouds.end(); ++it) {
        EPointCloudPtr mpc = *it;
        vector<EPolarPoint3D> vec = mpc->polars;
        for(unsigned int i = 0 ; i < mpc->polars.size(); ++i) {
            //LOG("%s pt: %s\n", LOG_START, vec[i].toString().c_str());
        	Point3D point(vec[i].x, vec[i].y, vec[i].z);
        	Colour colour = getEdgePointColour(point, mpc->polars[i].edgeType);
        	toRobot->cloud.push_back(point);
        	toRobot->colours.push_back(colour);
        }
    }

	// Add in holes
	for(vector<HoleExtPtr>::iterator it = holes.begin(); it != holes.end(); ++it) {
		HoleExtPtr h = *it;
		toRobot->cloud.push_back(Point3D(h->p.x, h->p.y, h->p.z));
		toRobot->colours.push_back(Colour(0, 255, 0));
	}

    return toRobot;
}

Colour HoleDetector::getEdgePointColour(Point3D& p, EdgeType edgeType) {
    Colour colour;
	if (edgeType == ET_NOT_EDGE) {
        colour.r = 0;
        colour.g = 0;
        colour.b = 0;
    } else if (sendAllEdges && edgeType == ET_EDGE) {
        colour.r = 0;
        colour.g = 0;
        colour.b = 255;
    } else if (edgeType == ET_HOLE_POS) {
        colour.r = 255;
        colour.g = 0;
        colour.b = 0;
    } else if (edgeType == ET_HOLE_NEG) {
        colour.r = 255;
        colour.g = 255;
        colour.b = 0;
    } else if (edgeType == ET_VERTICAL_EDGE) {
        colour.r = 255;
        colour.g = 0;
        colour.b = 255;
    }
	return colour;
}

void HoleDetector::publishHolesPC(vector<EPointCloudPtr>& polarClouds, vector<HoleExtPtr>& holes) {
	// Create colour point cloud
	PointCloudPtr toRobot = preparePCforRobot(polarClouds, holes);
	//LOG("%s \t Sending %ld points\n", LOG_START, toRobot->cloud.size());

	// Publish
	sensor_msgs::PointCloudPtr msg = toRobot->toROS1();
	holeCloudPub.publish(*msg);
}

/* VERTICAL EDGE DETECTION
void calculateBelowAbove(vector<EPolarPoint3D>& polars, double phi, unsigned int start,
                         unsigned int* below, unsigned int* above) {
    while (start < polars.size() && polars[start].phi < phi) {
        start++;
    }
    *below = start != 0 ? start - 1 : 0;
    *above = start != polars.size() ? start : start - 1;
}
void _HoleDetection::verticalEdgeEdgeDetection(vector<EPointCloud>& polarClouds, double neighbourhood) {
    // Sort clouds on theta value
    sort(polarClouds.begin(), polarClouds.end(), sortPCTheta);

    // Iterate through each Point Cloud
    for(unsigned int pcIndex = 1; pcIndex < polarClouds.size() - 1; pcIndex++) {
        EPointCloud& prev = polarClouds[pcIndex-1];
        EPointCloud& curr = polarClouds[pcIndex];
        EPointCloud& next = polarClouds[pcIndex+1];
        vector<EPolarPoint3D>& prevLP = prev->polars;
        vector<EPolarPoint3D>& currLP = curr->polars;
        vector<EPolarPoint3D>& nextLP = next->polars;

        // Move through each curr point
        unsigned int closestPrevAb = 0;
        unsigned int closestPrevBe = 0;
        unsigned int closestNextAb = 0;
        unsigned int closestNextBe = 0;
        for(unsigned int currIndex = 0; currIndex < polarClouds.size(); currIndex++) {
            EPolarPoint3D& currEPP = currLP[currIndex];

            // Update above/below indicies
            calculateBelowAbove(prevLP, currEPP.phi, closestPrevBe, &closestPrevBe, &closestPrevAb);
            calculateBelowAbove(nextLP, currEPP.phi, closestNextBe, &closestNextBe, &closestNextAb);

            // Get diffs
            double prevBelowDiff = fabs(currEPP.phi - prevLP[closestPrevBe].phi);
            double prevAboveDiff = fabs(currEPP.phi - prevLP[closestPrevAb].phi);
            double nextBelowDiff = fabs(currEPP.phi - nextLP[closestNextBe].phi);
            double nextAboveDiff = fabs(currEPP.phi - nextLP[closestNextAb].phi);

            // Get closest point
            double prevClosestDiff = prevBelowDiff <= prevAboveDiff ? prevBelowDiff : prevAboveDiff;
            double nextClosestDiff = nextBelowDiff <= nextAboveDiff ? nextAboveDiff : nextBelowDiff;

            // Determine if edge
            if (prevClosestDiff <= neighbourhood && nextClosestDiff <= neighbourhood) {
                double verticalDiff = fabs(prevClosestDiff - nextClosestDiff);
                if (verticalDiff >= edgeThreshold) {
                    if (currEPP.edgeType == ET_NOT_EDGE || currEPP.edgeType == ET_EDGE) {
                        currEPP.edgeType = ET_VERTICAL_EDGE;
                        currEPP.diff = verticalDiff;
                    }
                }
            }
        }
    }
}
*/

/* SEND/RECV Modes
// From configure()
	// Load mode
    std::string modeStr = config->getParam("mode", "all");
    if (modeStr == "all") {
        mode = HDM_ALL;
    } else if (modeStr == "send") {
        mode = HDM_SEND;
    } else if (modeStr == "receive") {
        mode = HDM_RECV;
    }

// From preparePCforRobot
    if (mode == HDM_ALL) {
        toRobot->frameID = POINTCLOUD_HOLEDETECTION;
    } else if (mode == HDM_SEND) {
        toRobot->frameID = POINTCLOUD_HOLEDETECTION_SEND;
    } else if (mode == HDM_RECV) {
        toRobot->frameID = POINTCLOUD_HOLEDETECTION_RECV;
    }

void HoleDetector::sendEdges(vector<EPointCloudPtr>& polarClouds) {
	// Create data buffer
    DataBuffer buffer = new _DataBuffer(POINTCLOUD_HOLEDETECTION_SEND);

    // Add all EPointClouds with their theta value and edges
    for(unsigned int ipc = 0; ipc != polarClouds.size(); ipc++) {
        EPointCloud& polarCloud = polarClouds[ipc];

        // Add theta value
        buffer->floats.push_back(polarCloud->theta);

        // For each edge add:
        //      - index in polarClouds
        //      - laser point
        for(unsigned int ipt = 0; ipt != polarCloud->polars.size(); ipt++) {
            // Index in pointClouds
            if (polarCloud->polars[ipt].edgeType == ET_EDGE) {
                buffer->doubles.push_back(ipc);

                // Laser Point
                buffer->doubles.push_back(polarCloud->laserPoints[ipt].x);
                buffer->doubles.push_back(polarCloud->laserPoints[ipt].y);
                buffer->doubles.push_back(polarCloud->laserPoints[ipt].z);

                // Polar Point
                //buffer->doubles.push_back(polarCloud->polars[ipt].x);
                //buffer->doubles.push_back(polarCloud->polars[ipt].y);
                //buffer->doubles.push_back(polarCloud->polars[ipt].z);
                buffer->doubles.push_back(polarCloud->polars[ipt].dist);
                buffer->doubles.push_back(polarCloud->polars[ipt].theta);
                buffer->doubles.push_back(polarCloud->polars[ipt].phi);
                buffer->doubles.push_back(polarCloud->polars[ipt].diff);
            }
        }
    }

    // Send data buffer
    //LOG("%s \t Sending data buffer: %s\n", LOG_START, buffer->description.c_str());
    robot->addDataBuffer(buffer);
}

void _HoleDetection::recieveEdges(DataBuffer buffer) {

	if (buffer->description == POINTCLOUD_HOLEDETECTION_SEND) {
        Lock lock(processMutex);
        //LOG("%s Got new Data buffer, parsing\n", LOG_START);

        polarClouds.clear();

        unsigned int doublesIndex = 0;
        int currentPC = -1;
        while (doublesIndex < buffer->doubles.size()) {
            int thisPC = (int) buffer->doubles[doublesIndex];

            // If pointCloud index has changed, create new cloud
            if (thisPC != currentPC) {
                polarClouds.push_back(new _EPointCloud());
                polarClouds.back()->theta = buffer->floats[thisPC];
                currentPC = thisPC;
            }

            // Load laser point
            Point3D laserPoint;
            laserPoint.x = buffer->doubles[doublesIndex + 1];
            laserPoint.y = buffer->doubles[doublesIndex + 2];
            laserPoint.z = buffer->doubles[doublesIndex + 3];
            polarClouds.back()->laserPoints.push_back(laserPoint);

            // Load polar point
            EPolarPoint3D polarPoint;
            //polarPoint.x        = buffer->doubles[doublesIndex + 4];
            //polarPoint.y        = buffer->doubles[doublesIndex + 5];
            //polarPoint.z        = buffer->doubles[doublesIndex + 6];
            polarPoint.dist     = buffer->doubles[doublesIndex + 4];
            polarPoint.theta    = buffer->doubles[doublesIndex + 5];
            polarPoint.phi      = buffer->doubles[doublesIndex + 6];
            polarPoint.diff     = buffer->doubles[doublesIndex + 7];
            polarPoint.edgeType = ET_EDGE;
            polarClouds.back()->polars.push_back(polarPoint);

            // Increment
            doublesIndex += 8;
        }

        // Notify semaphore of continue
        recvSemaphore.notify();
    }
}

*/

/* BIN BEHAVIOUR OF POLAR CO-ORDS
Bin::Bin() {
    totalDist = 0;
    averageDist = 0;
    edgeBin = false;
}

Bin::~Bin() {
}

PolarColouredPoint Bin::operator[](int i) {
    return polars[i];
}

double Bin::getAverageDist() {
    return averageDist;
}

double Bin::getTotalDist() {
    return totalDist;
}

bool Bin::isEdgeBin() {
    return edgeBin;
}

void Bin::add(PolarColouredPoint polar) {
    polars.push_back(polar);
    totalDist += polar.dist;
    averageDist = totalDist / polars.size();
}

int Bin::size() {
    return polars.size();
}

void Bin::clear() {
    polars.clear();
    totalDist = 0;
    averageDist = 0;
}

void Bin::setEdgeBin(bool edgeBin) {
    this->edgeBin = edgeBin;
}
*/

/*
inline int _HoleDetection::thetaToBinNo(double angle) {
    return floor(angle * noThetaBins / 360);
}

inline int _HoleDetection::phiToBinNo(double angle) {
    return floor(angle * noPhiBins / 360);
}

void _HoleDetection::binPolarPoints(vector<PolarColouredPoint>& points, Bins& bins) {
    int thetaBin, phiBin;

    // Iterate through each polar point
    for(vector<PolarColouredPoint>::iterator it = points.begin(); it != points.end(); ++it) {
        // Get bin No for each angle
        // Offset phi as it is between (-PI, PI]
        thetaBin = thetaToBinNo(it->theta);
        phiBin = phiToBinNo(it->phi + 180);
        //LOG("%sTheta/Phi: %lf, %lf \tTheta/Phi bins: %d, %d\n", LOG_START, it->theta, it->phi, thetaBin, phiBin);

        // Add point to the bin
        bins[thetaBin][phiBin]->add(*it);
    }
}

void _HoleDetection::cleanBins(Bins& bins) {
    for (int i = 0; i != noThetaBins; ++i) {
        for (int j = 0; j != noPhiBins; ++j) {
            bins[i][j]->clear();
        }
    }
}
*/

} // namespace casualty
