struct SmartPoint {
	//! The point
	Point3F p;
	//! The normal
	Point3F n;
	//! The average Euclidean distance to the neighbors
	double ad;
	//! User pointer
	void *up;
};

class SmartPointCloud : public QLinkedList<SmartPoint*> {
 
	//! Computes the normal vector of each point from its \a k nearest neighbors.
	void computeNormalVectors(int k=20);
 
	//! Segments the points by grouping all points with at least \a l neighbors within distance \a r and normal vector angle difference of \a a degrees. Removes clusters with less than \a nMin points.
	QList<SmartPointCloud*> segmentByNormalVectors(double aMax, double rMax, int nMin);
 
	//! Appends smart point cloud \a other to this one.
	SmartPointCloud& operator<<(const SmartPointCloud &other);
 
}

QList<SmartPointCloud*> SmartPointCloud::segmentByNormalVectors(double aMax, double rMax, int nMin) {
	if (aMax<=0)
		return QList<SmartPointCloud*>();
 
	qDebug("[SmartPointCloud] Segmenting smart point cloud by normal vectors...");
	time.start();
 
	//Create a kd-tree if necessary, allocate space, prepare parameters
	if (!kdTree)
		kdTree = createKdTreeFromPoints();
	ANNpointArray points = kdTree->thePoints();
	const int k = MIN(50,count()-1);
	ANNidxArray indices = new ANNidx[k];
	ANNdistArray distances = new ANNdist[k];
	double aThresh = cos(aMax/180.0*M_PI);
 
	//Make sure normal vectors have been estimated
	if (!normalVectorsComputed)
		computeNormalVectors(k);
 
	//Create a temporary array of indices and clean the user pointers
	SmartPoint **pArray = new SmartPoint*[count()];
	int i=0; 
	for (iterator p=begin(); p!=end(); p++) {
		pArray[i] = (*p);
		pArray[i]->up = NULL;
		i++;
	}
 
	int id=0;
	for (iterator p=begin(); p!=end(); p++) {
		//Current point
		SmartPoint *cp = (*p);
 
		//Get the nearest neighbors
		double r = MIN(rMax,cp->ad);
		kdTree->annkFRSearch(points[id],r*r,k,indices,distances);
 
		//Now cluster
		for (int j=0; j<k; j++) {
			if (indices[j] == ANN_NULL_IDX)
				break;
			if (fabs(cp->n.dot(pArray[indices[j]]->n)) < aThresh)
				continue;
			//Neighbor point
			SmartPoint *np = pArray[indices[j]];
			if (cp->up) {
				if (np->up) {
					//If points belong to the same cluster continue
					if (cp->up == np->up)
						continue;
					//Else merge the clusters (append the smaller to the larger)
					bool g = ((SmartPointCloud*)cp->up)->count() > ((SmartPointCloud*)np->up)->count();
					SmartPointCloud *pc1 = g ? (SmartPointCloud*)cp->up : (SmartPointCloud*)np->up;
					SmartPointCloud *pc2 = g ? (SmartPointCloud*)np->up : (SmartPointCloud*)cp->up;
					for (iterator p=pc2->begin(); p!=pc2->end(); p++)
						(*p)->up = pc1;
					(*pc1) << (*pc2);
					delete pc2;
				} else {
					//Neighbor point doesn't have a cluster, add it to this one
					np->up = cp->up;
					((SmartPointCloud*)cp->up)->append(np);
				}
			} else if (np->up) {
				//Add this point to neighbor cluster
				cp->up = np->up;
				((SmartPointCloud*)np->up)->append(cp);
			} else {
				//Both points don't have a cluster -> Create a new one
				SmartPointCloud *nc = new SmartPointCloud;
				cp->up = (void*)nc;
				nc->append(cp);
				np->up = (void*)nc;
				nc->append(np);
			}
		}
		id++;
	}
 
	//Gather the clusters and delete the ones with less than nMin points
	QList<SmartPointCloud*> clusters;
	for (iterator p=begin(); p!=end(); p++) {
		SmartPointCloud *cluster = (SmartPointCloud*)(*p)->up;
		if (!cluster)
			continue;
		if (clusters.contains(cluster)) {
			p = --erase(p);
			continue;
		}
		if (cluster->count() < nMin) {
			for (iterator cp=cluster->begin(); cp!=cluster->end(); cp++)
				(*cp)->up = NULL;
			delete cluster;
		} else {
			p = --erase(p);
			clusters.append(cluster);
		}
	}
 
	//The point array and kd-tree are no longer valid for this point cloud
	annDeallocPts(points);
	delete kdTree;
	kdTree = NULL;
 
	//Clean up and return
	delete indices;
	delete distances;
	delete inRange;
	delete pArray;
 
	qDebug("[SmartPointCloud] Done (%d ms). %d cluster(s).",time.elapsed(),clusters.count());
	return clusters;
}