text: /* 
   *  Copyright (c) 2011  Chen Feng (cforrest (at) umich.edu)
   *    and the University of Michigan
   *
   *  This program is free software; you can redistribute it and/or modify
   *  it under the terms of the GNU General Public License as published by
   *  the Free Software Foundation; either version 2 of the License, or
   *  (at your option) any later version.
   *
   *  This program is distributed in the hope that it will be useful,
   *  but WITHOUT ANY WARRANTY; without even the implied warranty of
   *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   *  GNU General Public License for more details.
   *
   */
  
  #include "stdio.h"
  #include "updator.h"
  #include "RandomSampler.h"
  #include "JLinkage.h"
  #include "VPPrimitive.h"
  #include "VPCluster.h"
  
  namespace VPCluster {
  	using namespace Updator;
  
  	struct LabelStat {
  		unsigned int cnt;
  		unsigned int id;
  		bool operator<(const LabelStat& rhs)
  		{
  			if(cnt!=rhs.cnt) return cnt>rhs.cnt;
  			return id>rhs.id;
  		}
  	};
  
  	/*void PrintHelp(){
  
  		printf("\n\n *** JLinkageLibClusterizeMex v.1.0 *** Part of the SamantHa Project");
  		printf("\n     Author roberto.toldo@univr.it - Vips Lab - Department of Computer Science - University of Verona(Italy) ");
  		printf("\n ***********************************************************************");
  		printf("\n Usage: [Labels, PS] = JLnkClusterize(Points, Models, InlierThreshold, ModelType, *KDTreeRange = -1*, *ExistingClusters = []*)");
  		printf("\n Input:");
  		printf("\n        Points - Input dataset (Dimension x NumberOfPoints)");
  		printf("\n        Models - Hypotesis generated by the JLinkageLibRandomSamplerMex(Dimension x NumberOfModels)");
  		printf("\n        InlierThreshold - maximum inlier distance point-model ");
  		printf("\n        ModelType - type of models extracted. Currently the model supported are: 0 - Planes 1 - 2dLines");
  		printf("\n        KDTreeRange(facultative) - Select the number of neighboards to use with in the agglomerative clustering stage. ( if <= 0 all the points are used; n^2 complexity)");
  		printf("\n        ExistingClusters(facultative) - Already existing clusters, Logical Matrix containing Pts X NCluster");
  		printf("\n Output:");
  		printf("\n        Labels - Belonging cluster for each point");
  		printf("\n        PS - Preference set of resulting clusters");
  		printf("\n");
  
  	}*/
  
  
  	// Function pointers
  	std::vector<float>  *(*mGetFunction)(const std::vector<sPt *> &nDataPtXMss, const std::vector<unsigned int>  &nSelectedPts);
  	float (*mDistanceFunction)(const std::vector<float>  &nModel, const std::vector<float>  &nDataPt);
  
  	//// Output arguments
  	// Arg 0, NPoints x NSample logical preference set matrix
  	//bool *mPSMatrix;
  
  	unsigned int run(
  		//// OUTPUT
  		std::vector<unsigned int>& Lables,
  		std::vector<unsigned int>& LableCount,
  		//// Input arguments
  		// Arg 0, points
  		std::vector<std::vector<float> *> *mDataPoints,
  		// Arg 1, Models
  		std::vector<std::vector<float> *> *mModels,
  		// Arg 2, InliersThreshold
  		float mInlierThreshold,
  		// Arg 3, type of model: 0 - Planes 1 - 2dLines
  		unsigned int /*mModelType*/,
  		// ----- facultatives
  		// Arg 4, Select the KNN number of neighboards that can be merged. ( = 0 if all the points are used; n^2 complexity)
  		int mKDTreeRange,
  		// Arg 5, Already existing clusters, Logical Matrix containing Pts X NCluster");
  		std::vector<std::vector<unsigned int> > mExistingClusters
  		) 
  	{
  		// arg2 : InlierThreshold
  		if(mInlierThreshold <= 0.0f) {
  			printf("Invalid InlierThreshold");
  			return 0;
  		}
  
  		// arg3 : modelType;
  		/*switch(mModelType){
  		case MT_PLANE: 
  			mGetFunction = GetFunction_Plane;
  			mDistanceFunction = DistanceFunction_Plane;
  			break;
  		case MT_LINE: 
  			mGetFunction = GetFunction_Line;
  			mDistanceFunction = DistanceFunction_Line;
  			break;
  		case MT_VP:*/
  			mGetFunction = GetFunction_VP;
  			mDistanceFunction = DistanceFunction_VP;
  		/*	break;
  		default: printf("Invalid model type"); return 0; break;
  		}*/
  
  		printf("Initializing Data... \n");
  		// Compute the jLinkage Clusterization
  		JLinkage mJLinkage(mDistanceFunction, mInlierThreshold, 
  			(unsigned int)mModels->size(), true, 
  			(unsigned int)((*mDataPoints)[0])->size(), mKDTreeRange);
  
  		std::vector<const sPtLnk *> mPointMap(mDataPoints->size());	
  
  		std::list<sClLnk *> mClustersList;
  
  		unsigned int counter = 0;
  		InitializeWaitbar("Loading Models ");
  		for(unsigned int nModelCount = 0; nModelCount < mModels->size(); nModelCount++){
  			mJLinkage.AddModel(((*mModels)[nModelCount]));
  			++counter;
  			UpdateWaitbar((float)counter/(float)mModels->size());
  		}
  		CloseWaitbar();
  
  		counter = 0;
  		InitializeWaitbar("Loading Points ");
  		for(std::vector<std::vector<float> *>::iterator iterPts = mDataPoints->begin();
  				iterPts != mDataPoints->end(); ++iterPts ){
  			mPointMap[counter] = mJLinkage.AddPoint(*iterPts);
  			++counter;
  			UpdateWaitbar((float)counter/(float)mDataPoints->size());
  		}
  		CloseWaitbar();
  
  		if(mExistingClusters.size() > 0){
  			printf("\tLoading Existing Models \n");
  			for(int i=0; i < (int)mExistingClusters.size(); ++i){
  				if(!mJLinkage.ManualClusterMerge(mExistingClusters[i])) {
  					printf("Invalid Existing cluster matrix");
  					return 0;
  				}
  			}
  		}
  
  		InitializeWaitbar("J-Clusterizing ");
  		mClustersList = mJLinkage.DoJLClusterization(UpdateWaitbar);
  		CloseWaitbar();
  
  		// Write output
  		// 	plhs[0] = mxCreateDoubleMatrix(1,mDataPoints->size(), mxREAL);
  		// 	double *mTempUintPointer = (double *)mxGetPr(plhs[0]);
  
  		std::vector<LabelStat> stats;
  
  		unsigned int counterCl = 0;
  		Lables.clear();
  		Lables.resize(mDataPoints->size(),10);
  		for(std::list<sClLnk *>::iterator iterCl = mClustersList.begin(); iterCl != mClustersList.end(); ++iterCl){
  			unsigned int cnt=0;
  			for(std::list<sPtLnk *>::iterator iterPt = (*iterCl)->mBelongingPts.begin(); iterPt != (*iterCl)->mBelongingPts.end(); ++iterPt){
  				unsigned int counterPt = 0;
  				for(std::vector<const sPtLnk *>::iterator iterPtIdx = mPointMap.begin(); iterPtIdx != mPointMap.end(); ++iterPtIdx){
  					if((*iterPt) == (*iterPtIdx)){
  						//mTempUintPointer[counterPt] = counterCl;
  						Lables[counterPt] = counterCl;
  						++cnt;
  						break;
  					}
  					++counterPt;
  				}
  			}
  			LabelStat s;
  			s.cnt = cnt;
  			s.id = counterCl;
  			stats.push_back(s);
  			++counterCl;
  		}
  
  		std::sort(stats.begin(),stats.end());
  		std::vector<unsigned int> Idx(stats.size());
  		LableCount.clear();
  		for(int i=0; i<(int)stats.size(); ++i) {
  			Idx[ stats[i].id ] = i;
  			LableCount.push_back(stats[i].cnt);
  		}
  
  		unsigned int lsize = (int)Lables.size();
  		for(int i=0; i<(int)lsize; ++i) {
  			unsigned int old = Lables[i];
  			Lables[i] = Idx[old];
  		}
  
  		return (int)stats.size();
  
  	}
  
  }
  