#ifdef NICE_USELIB_OPENMP
#include <omp.h>
#endif

#include "SemSegRegionBased.h"

#include <iostream>

#include "objrec/cbaselib/CachedExample.h"
#include "objrec/classifier/fpclassifier/randomforest/FPCRandomForests.h"
#include "objrec/classifier/fpclassifier/logisticregression/FPCSMLR.h"

#include <objrec/iclassifier/icgeneric/CSGeneric.h>
#include "objrec/features/fpfeatures/PixelPairFeature.h"
#include "objrec/classifier/genericClassifierSelection.h"

#include "SemSegTools.h"

#include "objrec/segmentation/RSMeanShift.h"
#include "objrec/segmentation/RSCache.h"
#include "objrec/segmentation/RSGraphBased.h"

#include "objrec/baselib/Globals.h"

#include <objrec/cbaselib/VectorFeature.h>

#include "objrec/features/fpfeatures/SparseVectorFeature.h"
#include "objrec/features/localfeatures/LFColorWeijer.h"
#include "objrec/features/localfeatures/LFColorSande.h"
#include "objrec/features/localfeatures/LocalFeatureSift.h"
#include "objrec/features/localfeatures/LocalFeatureOpponnentSift.h"
#include "objrec/features/localfeatures/LocalFeatureLFInterface.h"
#include "objrec/features/localfeatures/LocalFeatureRGBSift.h"
#include "objrec/features/localfeatures/LFCache.h"

#include "objrec/features/regionfeatures/RFColor.h"
#include "objrec/features/regionfeatures/RFHoG.h"
#include "objrec/features/regionfeatures/RFBoV.h"
#include "objrec/features/regionfeatures/RFBoVCodebook.h"
#include "objrec/features/regionfeatures/RFCsurka.h"

#include "objrec/iclassifier/codebook/CodebookRandomForest.h"

#include "objrec/math/cluster/GMM.h"

#undef DEMO
#undef WRITEFEATS

using namespace OBJREC;
using namespace std;
using namespace NICE;

#define DEBUG_PRINTS

SemSegRegionBased::SemSegRegionBased(const Config *c, const MultiDataset *md)
		: SemanticSegmentation(c, &(md->getClassNames("train")))
{
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased Constructor starts" << endl;
	#endif
	conf = c;	
	
	save_cache = conf->gB("SemSegRegion", "save_cache", true);
	read_cache = conf->gB("SemSegRegion", "read_cache", false);
	classifiercache = conf->gS("SemSegRegion", "cache", "classifier.data");
	cache = conf->gS("cache", "root", "tmp/");
	bool colorw = conf->gB("SemSegRegion", "colorw", false);
	bool bov = conf->gB("SemSegRegion", "bov", false);
	bool hog = conf->gB("SemSegRegion", "hog", false);
	bool structf = conf->gB("SemSegRegion", "struct", false);
	string classifiertype = conf->gS("SemSegRegion", "classifier", "RF");
	bool usegcopt = conf->gB("SemSegRegion", "gcopt", false);
	bool bovmoosmann = conf->gB("SemSegRegion", "bovmoosmann", false);
	bool csurka = conf->gB("SemSegRegion", "csurka", false);
	
	if(colorw)
	{
		LocalFeature *lfcw = new LFColorWeijer(conf);
		rfc = new RFColor(conf, lfcw);
	}
	else
	{
		rfc = NULL;
	}
	
	if(hog)
	{
		rfhog = new RFHoG(conf);
	}
	else
	{
		rfhog = NULL;
	}
	
	if(structf)
	{
		rfstruct = new RFStruct(conf);
	}
	else
	{
		rfstruct = NULL;
	}
	
	LocalFeature *lfcache = NULL;
	
	if(bov || bovmoosmann || csurka)
	{
		string ftype = conf->gS("BOV", "feature", "sift");
		
		siftFeats = NULL;
		
		if(ftype == "sift")
		{
			siftFeats = new LocalFeatureSift ( conf );
			lfcache = new LFCache(conf, siftFeats);
		}
		
		if(ftype == "osift")
		{
			siftFeats = new LocalFeatureOpponnentSift ( conf );
			lfcache = new LFCache(conf, siftFeats);
		}
		
		if(ftype == "rsift")
		{
			siftFeats = new LocalFeatureRGBSift ( conf );
			lfcache = new LFCache(conf, siftFeats);
		}
		
		if(ftype == "sande")
		{
			LocalFeatureRepresentation *sande = new LFColorSande(conf, "LFColorSandeTrain");
			siftFeats = new LocalFeatureLFInterface ( conf, sande);
			
			LocalFeatureRepresentation *sande2 = new LFColorSande(conf, "LFColorSandeTest");
			LocalFeature *siftFeats2 = new LocalFeatureLFInterface ( conf, sande2);
			lfcache = new LFCache(conf, siftFeats2);
		}
		
		if(siftFeats == NULL)
		{
			throw "please choose one of the following features für BOV: osift, rsift, sift, sande";
		}
	}

	if(csurka)
	{
		rfCsurka = new RFCsurka(conf, lfcache);
	}
	else
	{
		rfCsurka = NULL;
	}

	if(bov)
	{
		rfbov = new RFBoV (conf, lfcache);
	}
	else 
	{
		rfbov = NULL;
	}
	
	if(bovmoosmann)
	{
		rfbovcrdf = new RFBoVCodebook (conf, lfcache);
	}
	else
	{
		rfbovcrdf = NULL;
	}
	
	// setting classifier
	fpc = NULL;
	vclassifier = NULL;
	
	if(classifiertype == "RF")
	{
		fpc = new FPCRandomForests(conf, "ClassifierForest");
	}
	else if(classifiertype == "SMLR")
	{
		fpc = new FPCSMLR(conf, "ClassifierSMLR");
	}
	else if(classifiertype == "VECC")
	{
		vclassifier = CSGeneric::selectVecClassifier ( conf, "vecClassifier" );
	}
	else
	{
		throw "classifiertype not (yet) supported";
	}
	
	if(fpc != NULL)
		fpc->setMaxClassNo(classNames->getMaxClassno());
	else if(vclassifier != NULL)
		vclassifier->setMaxClassNo(classNames->getMaxClassno());

	cn = md->getClassNames ( "train" );
	
	// setting segmentation method
	RegionSegmentationMethod *tmprsm = new RSMeanShift(conf);
	rsm = new RSCache ( conf, tmprsm );
	//rsm = new RSGraphBased(conf);

	// use global optimization (MRF)
	if(usegcopt)
		gcopt = new PPGraphCut ( conf );
	else
		gcopt = NULL;
	
	classifiercache = cache+classifiercache;
	
	// read training data or start training
	if (read_cache)
	{
		fprintf(stderr, "SemSegRegion:: Reading classifier data from %s\n", cache.c_str());
		if(fpc != NULL)
			fpc->read(classifiercache);
		else if(vclassifier != NULL)
			vclassifier->read(classifiercache);
		
		if(rfCsurka != NULL)
		{
			bool usegmm = conf->gB("Csurka", "usegmm", false);
			bool usepca = conf->gB("Csurka", "usepca", false);
			
			if(usepca || usegmm)
			{
				RFCsurka *_rfcsurka = dynamic_cast< RFCsurka * >(rfCsurka);
				
				if(usepca)
				{
					int pcadim = conf->gI("Csurka", "pcadim", 100);
					PCA *pca = new PCA(pcadim);
					string pcadst = cache+"/csurka.pca";

					if (!FileMgt::fileExists(pcadst))
					{
						throw(pcadst+" not found");
					}
					else
					{
						pca->read ( pcadst );
					}
					
					_rfcsurka->setPCA(pca);
				}
				
				if(usegmm)
				{
					int gaussians = conf->gI("Csurka", "gaussians", 1024);
					GMM *g = new GMM( conf, gaussians );
					string gmmdst = cache+"/csurka.gmm";
					
					if ( !g->loadData ( cache+"/gmmSIFT" ) )
					{
						throw(gmmdst+" not found");
					}

					_rfcsurka->setGMM(g);
				}
			}
		}
		
		if(rfbov != NULL)
		{
			RFBoV *rfbovdyn = dynamic_cast< RFBoV * >(rfbov);
			
			int gaussians = conf->gI("SIFTTrain", "gaussians", 512);
			
			GMM *g = new GMM( conf, gaussians );
			PCA *pca = new PCA(100);
			string pcadst = cache+"/bov.pca";
	
			if ( !g->loadData ( cache+"/gmmSIFT" ) || !FileMgt::fileExists(pcadst))
			{
				throw("pca or gmm not found");
			}
			else
			{
				pca->read ( pcadst );
			}
			rfbovdyn->setPCA(pca);
			rfbovdyn->setGMM(g);
		}
		
		fprintf(stderr, "SemSegRegion:: successfully read\n");
	}
	else
	{
		train(md);
	}
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased Constructor finished" << endl;
	#endif
}

SemSegRegionBased::~SemSegRegionBased()
{
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased Destructor starts" << endl;
	#endif
	if (fpc != NULL)
	{
		delete fpc;
	}
	if(vclassifier != NULL)
	{
		delete vclassifier;
	}
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased Destructor finished" << endl;
	#endif
}

void SemSegRegionBased::train(const MultiDataset *md)
{
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::train starts" << endl;
	#endif
	
	Examples examples;
	examples.filename = "training";
	
	const LabeledSet train = * ( *md ) ["train"];
	
	set<int> forbidden_classes;
	
	std::string forbidden_classes_s = conf->gS ( "analysis", "donttrain", "" );
	if ( forbidden_classes_s == "" )
	{
		forbidden_classes_s = conf->gS ( "analysis", "forbidden_classes", "" );
	}
	
	cn.getSelection ( forbidden_classes_s, forbidden_classes );

	if(gcopt != NULL)
		gcopt->setClassNo ( cn.numClasses() );
	
	LabeledSet::Permutation perm;

	train.getPermutation(perm);

	learnHighLevel(perm);

	//FIXME:Moosmann
	
	int imgcounter = 0;

	vector<vector<FeatureType> > feats;
	// loop over all training images

	for ( LabeledSet::Permutation::const_iterator i = perm.begin();
			 i != perm.end(); i++,imgcounter++ )
	{
		const string fn = i->second->img();
		Globals::setCurrentImgFN ( fn );
		cout << fn << endl;
		NICE::ColorImage cimg(fn);
		NICE::Matrix mask;
		RegionGraph rg;
		rsm->getGraphRepresentation(cimg, mask, rg);
		
#ifdef DEMO
		rsm->visualizeGraphRepresentation(cimg, mask);
#endif

		// get label
		const LocalizationResult *locResult = i->second->localization();
		NICE::Image pixelLabels (cimg.width(), cimg.height());
		pixelLabels.set(0);
		locResult->calcLabeledImage ( pixelLabels, ( *classNames ).getBackgroundClass() );
		getRegionLabel(mask, rg, pixelLabels);
		
		getFeats(cimg, mask, rg, feats);
		
//#pragma omp critical
		for(int i = 0; i < rg.size(); i++)
		{
			int classno = rg[i]->getLabel();
			Example example;
			example.position = imgcounter;
			examples.push_back (pair<int, Example> ( classno, example ));
		}
//#pragma omp critical
		if ( gcopt !=NULL )
			gcopt->trainImage ( rg );

	}
cout << "train classifier starts" << endl;
	trainClassifier(feats, examples);
cout << "train classifier finished" << endl;

	if ( gcopt != NULL )
		gcopt->finishPP ( cn );

	// clean up
	/*for(int i = 0; i < (int) examples.size(); i++)
	{
		examples[i].second.clean();
	}*/
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::train finished" << endl;
	#endif
}

void SemSegRegionBased::getRegionLabel(NICE::Matrix &mask, RegionGraph &rg, NICE::Image &pixelLabels)
{	
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::getRegionLabel starts" << endl;
	#endif
	vector<vector<int> > hists;
	int regionsize = rg.size();
	int xsize = pixelLabels.width();
	int ysize = pixelLabels.height();

	for ( int i = 0; i < regionsize; i++ )
	{
		vector<int> hist ( cn.numClasses(), 0 );
		hists.push_back ( hist );
	}

	for ( int x = 0; x < xsize; x++ )
	{
		for ( int y = 0; y < ysize; y++ )
		{
			int numb = mask(x,y);
			hists[numb][pixelLabels.getPixel(x,y)]++;
		}
	}

	for ( int i = 0; i < regionsize; i++ )
	{
		int maxval = -numeric_limits<int>::max();
		int smaxval = -numeric_limits<int>::max();
		int maxpos = -1;
		int secondpos = -1;
		for ( int k = 0; k < ( int ) hists[i].size(); k++ )
		{
			if ( maxval < hists[i][k] )
			{
				secondpos = maxpos;
				smaxval = maxval;
				maxval = hists[i][k];
				maxpos = k;
			}
			else
			{
				if ( smaxval < hists[i][k] )
				{
					smaxval = hists[i][k];
					secondpos = k;
				}
			}
		}

		// FIXME: das für alle verbotenen Klassen einbauen
		//if ( forbidden_classes.find ( classno ) != forbidden_classes.end() )

		if ( cn.text ( maxpos ) == "various" && smaxval > 0)
			rg[i]->setLabel(secondpos);
		else
			rg[i]->setLabel(maxpos);
	}
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::getRegionLabel finished" << endl;
	#endif
}

void SemSegRegionBased::getExample(const vector<vector<FeatureType> > &feats, Examples &examples)
{
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::getExample starts" << endl;
	#endif
	
	for(int j = 0; j < (int)feats.size(); j++)
	{
		int counter = 0;
		for(int i = 0; i < (int)feats[0].size(); i++, counter++)
		{
			if(examples[counter].second.vec == NULL)
			{
				NICE::Vector *vec = new NICE::Vector(feats[j][i].getVec());
				examples[counter].second.vec = vec;
			}
			else
			{
				examples[counter].second.vec->append(feats[j][i].getVec());
			}
		}
	}
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::getExample finished" << endl;
	#endif
}

void SemSegRegionBased::getFeaturePool( const vector<vector<FeatureType> > &feats, FeaturePool &fp)
{
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::getFeaturePool starts" << endl;
	#endif
	
	int olddim = 0;
	int fulldim = 0;
	
	for(int j = 0; j < (int)feats.size(); j++)
	{
		fulldim += feats[j][0].getDim();
	}
	
	for(int j = 0; j < (int)feats.size(); j++)
	{
		int dimension = feats[j][0].getDim();
		for ( int i = olddim ; i < olddim+dimension ; i++ )
		{
			VectorFeature *f = new VectorFeature ( fulldim );
			f->feature_index = i;
			fp.addFeature(f, 1.0 / dimension);
		}
		olddim += dimension;
	}
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::getFeaturePool finished" << endl;
	#endif
}

void SemSegRegionBased::trainClassifier(vector<vector<FeatureType> > &feats, Examples & examples)
{
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::trainClassifier starts" << endl;
	#endif
	assert (feats.size() > 0);
	assert (feats[0].size() > 0);

	// delete nonrelevant features
	for(int i = (int)examples.size()-1; i >= 0; i--)
	{
		if(cn.text ( examples[i].first ) == "various")
		{
			examples.erase(examples.begin()+i);
			for(int k = 0; k < (int)feats.size(); k++)
			{
				feats[k].erase(feats[k].begin()+i);
			}
		}
	}
	
#ifdef WRITEFEATS
	// mermale in datei schreiben
	ofstream fout( "trainfeats", ios_base::out );
	//vector<int> ccounter(cn.getMaxClassno(),0);
	//int maxv = 100;
	for(int i = 0; i < (int)examples.size(); i++)
	{
		//if(ccounter[examples[i].first]++ < maxv)
		//{
			fout << examples[i].first << " ";
			for(int j = 0; j < (int)feats.size(); j++)
			{
				for(int k = 0; k < feats[j][i].getDim(); k++)
				{
					fout << feats[j][i].get(k) << " ";
				}
			}
			fout << endl;
		//}
	}
#endif
	
	if(fpc != NULL)
	{
		FeaturePool fp;
		getExample(feats, examples);
		getFeaturePool(feats, fp);

		fpc->train ( fp, examples );

		fp.destroy();

		if(save_cache)
		{
			fpc->save(classifiercache);
		}

//#pragma omp parallel for		
		for(int i = 0; i < (int)examples.size(); i++)
		{
			if(examples[i].second.vec != NULL)
			{
				delete examples[i].second.vec;
				examples[i].second.vec = NULL;
			}
		}

	}
	else if(vclassifier != NULL)
	{
		LabeledSetVector lsv;
		
//#pragma omp parallel for
		for(int i = 0; i < (int)feats[0].size(); i++)
		{
			NICE::Vector *v = new NICE::Vector(feats[0][i].getVec());
			for(int j = 1; j < (int)feats.size(); j++)
			{
				v->append(feats[j][i].getVec());
			}
//#pragma omp critical
			lsv.add_reference (examples[i].first, v );
		}

		vclassifier->teach(lsv);
		vclassifier->finishTeaching();
		lsv.clear();
		if(save_cache)
		{
			vclassifier->save(classifiercache);
		}
	}
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::trainClassifier finished" << endl;
	#endif
}

void SemSegRegionBased::classify(const vector<vector<FeatureType> > &feats, Examples &examples, vector<vector<double> > &probs)
{
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::classify starts" << endl;
	#endif
	for(int i = 0; i < (int)feats[0].size(); i++)
	{
		Example example;
		examples.push_back (pair<int, Example> ( -1, example ) );
	}
	
	getExample(feats, examples);
	
	int nbcl = classNames->getMaxClassno() + 1;
	
	for(int i = 0; i < (int)examples.size(); i++)
	{
		vector<double> p;
		ClassificationResult r;
			
		if(fpc != NULL)
		{
			r = fpc->classify ( examples[i].second );
		}
		else if(vclassifier != NULL)
		{
 			r = vclassifier->classify(*(examples[i].second.vec));
		}
	
		for ( int j = 0 ; j < nbcl; j++ )
		{
			p.push_back(r.scores[j]);
		}

		probs.push_back(p);
	}
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::classify finished" << endl;
	#endif
}

void SemSegRegionBased::semanticseg(CachedExample *ce, NICE::Image & segresult,	GenericImage<double> & probabilities)
{
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::semanticseg starts" << endl;
	#endif
	int xsize, ysize;

	ce->getImageSize ( xsize, ysize );

	probabilities.reInit ( xsize, ysize, classNames->getMaxClassno() +1, true/*allocMem*/ );
	std::string currentFile = Globals::getCurrentImgFN();
	NICE::ColorImage cimg(currentFile);

	NICE::Matrix mask;
	RegionGraph rg;
	rsm->getGraphRepresentation(cimg, mask, rg);
#ifdef DEMO
	rsm->visualizeGraphRepresentation(cimg, mask);
#endif
	vector<vector<FeatureType> > feats;

	getFeats(cimg, mask, rg, feats);

#ifdef WRITEFEATS	
	getRegionLabel(mask, rg, segresult);
	ofstream fout( "testfeats", ios_base::app );

	for(int i = 0; i < (int)rg.size(); i++)
	{
		fout << rg[i]->getLabel() << " ";
		for(int j = 0; j < (int)feats.size(); j++)
		{
			for(int k = 0; k < feats[j][i].getDim(); k++)
			{
				fout << feats[j][i].get(k) << " ";
			}
		}
		fout << endl;
	}
#endif
	
	segresult = NICE::Image(xsize, ysize);
	segresult.set(0);
	
	Examples examples; 
	
	vector<vector<double> > probs;

	classify(feats, examples, probs);

	labelRegions(rg, probs);

	if ( gcopt != NULL )
		gcopt->optimizeImage ( rg, probs );

	labelImage(segresult, mask, rg);
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::semanticseg finished" << endl;
	#endif
}

void SemSegRegionBased::labelRegions(RegionGraph &rg, vector<vector<double> > &probs)
{
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::labelRegions starts" << endl;
	#endif
	for(int i = 0; i < rg.size(); i++)
	{
		int bestclass = -1;
		double bestval = -numeric_limits<int>::max();
		for(int j = 0; j < (int)probs[i].size(); j++)
		{
			if(bestval < probs[i][j])
			{
				bestval = probs[i][j];
				bestclass = j;
			}
		}
		rg[i]->setLabel(bestclass);
	}
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::labelRegions finished" << endl;
	#endif
}

void SemSegRegionBased::labelImage(NICE::Image &segresult, NICE::Matrix &mask,RegionGraph &rg)
{
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::labelImage starts" << endl;
	#endif
	for(int y = 0; y < segresult.height(); y++)
	{
		for(int x = 0; x < segresult.width(); x++)
		{
			int r = (int)mask(x,y);
			segresult.setPixel(x,y,rg[r]->getLabel());
		}
	}
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::labelImage finished" << endl;
	#endif
}

void SemSegRegionBased::getFeats(const NICE::ColorImage &cimg, const NICE::Matrix &mask, const RegionGraph &rg, vector<vector< FeatureType> > &feats) const
{	
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::getFeats starts" << endl;
	#endif
	string fn = Globals::getCurrentImgFN();	
	NICE::Image img(fn);
	int featnb = 0;

	const int rgcount = rg.size();
	if(rfc != NULL)
	{
		if((int)feats.size() <= featnb)
		{
			vector<FeatureType> ftv;
			feats.push_back(ftv);
		}

		VVector features;
		rfc->extractRGB ( cimg, rg, mask, features );
		
		assert((int)features.size() == rgcount);
		
		for(int j = 0; j < (int)features.size(); j++)
		{
			feats[featnb].push_back(FeatureType(features[j]));
		}
#ifdef DEMO
		LFColorWeijer lfc(conf);
		lfc.visualizeFeatures (cimg);
#endif
		
		featnb++;
	}

	if(rfbov != NULL)
	{
		if((int)feats.size() <= featnb)
		{
			vector<FeatureType> ftv;
			feats.push_back(ftv);
		}

		VVector features;
		rfbov->extractRGB ( cimg, rg, mask, features );
		
		assert((int)features.size() == rgcount);
		
		for(int j = 0; j < (int)features.size(); j++)
		{
			feats[featnb].push_back(FeatureType(features[j]));
		}
		
		featnb++;
	}

	if(rfhog != NULL)
	{
		if((int)feats.size() <= featnb)
		{
			vector<FeatureType> ftv;
			feats.push_back(ftv);
		}

		VVector features;

		rfhog->extractRGB ( cimg, rg, mask, features );

		assert((int)features.size() == rgcount);

		for(int j = 0; j < (int)features.size(); j++)
		{
			feats[featnb].push_back(FeatureType(features[j]));
		}

		featnb++;
	}

	if(rfstruct != NULL)
	{
		if((int)feats.size() <= featnb)
		{
			vector<FeatureType> ftv;
			feats.push_back(ftv);
		}
		
		VVector features;
		rfstruct->extractRGB ( cimg, rg, mask, features );
		
		for(int j = 0; j < (int)features.size(); j++)
		{
			feats[featnb].push_back(FeatureType(features[j]));
		}
		
		featnb++;
	}

	if(rfbovcrdf != NULL)
	{
		if((int)feats.size() <= featnb)
		{
			vector<FeatureType> ftv;
			feats.push_back(ftv);
		}

		VVector features;
		rfbovcrdf->extractRGB ( cimg, rg, mask, features );

		assert((int)features.size() == rgcount);
		
		for(int j = 0; j < (int)features.size(); j++)
		{
			feats[featnb].push_back(FeatureType(features[j]));
		}
		
		featnb++;
	}

	if(rfCsurka != NULL)
	{
		if((int)feats.size() <= featnb)
		{
			vector<FeatureType> ftv;
			feats.push_back(ftv);
		}

		VVector features;

		rfCsurka->extractRGB ( cimg, rg, mask, features );

		assert((int)features.size() == rgcount);

		for(int j = 0; j < (int)features.size(); j++)
		{
			feats[featnb].push_back(FeatureType(features[j]));
		}

		featnb++;

	}
	
	/* Dummy for new features:
	if(siftFeats != NULL)
	{
		if((int)feats.size() <= featnb)
		{
			vector<FeatureType> ftv;
			feats.push_back(ftv);
		}
			
		featnb++;
	}
	*/
	
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::getFeats finished" << endl;
	#endif
}

void SemSegRegionBased::computeLF(LabeledSet::Permutation perm, VVector &feats, vector<int> &label, Examples &examples, int mode)
{
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::computeLF starts" << endl;
	#endif
	string sscales = conf->gS("SIFTTrain", "scales", "1+2.0+3.0");
	int grid = conf->gI("SIFTTrain", "grid", 20);
	double fraction = conf->gD("SIFTTrain", "fraction", 1.0);

	set<int> forbidden_classes;

	std::string forbidden_classes_s = conf->gS ( "analysis", "donttrain", "" );
	if ( forbidden_classes_s == "" )
	{
		forbidden_classes_s = conf->gS ( "analysis", "forbidden_classes", "" );
	}
	cn.getSelection ( forbidden_classes_s, forbidden_classes );
	cerr << "forbidden: " << forbidden_classes_s << endl;
				
	vector<double> scales;
	string::size_type pos = 0;
	string::size_type oldpos = 0;
	while(pos != string::npos)
	{
		pos = sscales.find("+", oldpos);
		string val;
		if(pos == string::npos)
			val = sscales.substr(oldpos);
		else
			val = sscales.substr(oldpos, pos-oldpos);
		double d = atof(val.c_str());
		scales.push_back(d);
		oldpos = pos+1;
	}

	int fsize = 0;
	
	string save = cache+"/siftTRAIN.dat";
	string savep = cache +"/siftPostions.dat";

	if(!FileMgt::fileExists(save) || !FileMgt::fileExists(savep))
	{
//FIXME: entfernen
//		vector<int> counter(9,0);
		for ( LabeledSet::Permutation::const_iterator i = perm.begin();
					i != perm.end(); i++ )
		{
			const string fn = i->second->img();
			Globals::setCurrentImgFN ( fn );

			NICE::Image img(fn);
			NICE::ColorImage cimg(fn);
			VVector features;
			VVector positions;

			int x0 = grid/2;
			for(int y = 0; y < (int)img.height(); y+=grid)
			{
				for(int x = x0; x < (int)img.width(); x+=grid)
				{
					for(int s = 0; s < (int)scales.size(); s++)
					{
						double r = (double)rand()/(double)RAND_MAX;
						if( r < fraction)
						{
							fsize++;
							NICE::Vector vec(3);
							vec[0] = x;
							vec[1] = y;
							vec[2] = scales[s];
							positions.push_back(vec);
						}
					}
				}
				if(x0 == 0)
				{
					x0 = grid/2;
				}
				else
				{
					x0 = 0;
				}
			}

			siftFeats->getDescriptors(cimg, positions, features);

			assert(positions.size() == features.size());
			
			const LocalizationResult *locResult = i->second->localization();
			NICE::Image pixelLabels (cimg.width(), cimg.height());
			pixelLabels.set(0);
			locResult->calcLabeledImage ( pixelLabels, ( *classNames ).getBackgroundClass() );

			for(int i = 0; i < (int)features.size(); i++)
			{
				int classno = pixelLabels(positions[i][0],positions[i][1]);
//				if ( cn.text ( classno ) == "various")
//					continue;

				if ( forbidden_classes.find ( classno ) != forbidden_classes.end() )
					continue;

//				counter[classno]++;
				label.push_back(classno);
				feats.push_back(features[i]);
			}
			assert(label.size() == feats.size());
		}
/*		cout << "samples for class: " << endl;
		for(int i = 0; i < 9; i++)
		{
			cout << i << ": " << counter[i] << endl;
		}
*/
		feats.save(save,1);
		ofstream lout(savep.c_str(),ios_base::out);
		for(uint i = 0; i < label.size(); i++)
		{
			lout << label[i] << " ";
		}
		lout.close();
	}
	else
	{
		feats.read(save,1);

		ifstream lin(savep.c_str(), ios_base::in);
		label.clear();
		for(int i = 0; i < (int)feats.size(); i++)
		{
			int l;
			lin >> l;
			label.push_back(l);
		}		
	}
	
	if(mode == 1)
	{
		convertVVectorToExamples(feats,examples, label);
	}
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::computeLF finished" << endl;
	#endif
}

void SemSegRegionBased::learnHighLevel(LabeledSet::Permutation perm)
{
	#ifdef DEBUG_PRINTS
	cout << "SemSegRegionBased::learnHighLevel starts" << endl;
	#endif
	srand ( time (NULL) );
	
	if(rfbov != NULL || rfbovcrdf != NULL || rfCsurka != NULL)
	{		
		if(rfbov != NULL)
		{
			RFBoV *rfbovdyn = dynamic_cast< RFBoV * >(rfbov);
			
			int gaussians = conf->gI("SIFTTrain", "gaussians", 512);
			int pcadim = conf->gI("SIFTTrain", "pcadim", 50);
			
			GMM *g = new GMM( conf, gaussians );
			PCA *pca = new PCA(pcadim);
			string pcadst = cache+"/pca.txt";
			
			if ( !g->loadData ( cache+"/gmmSIFT" ) || !FileMgt::fileExists( pcadst) )
			{
				VVector feats;
				vector<int> label;
			
				Examples ex;
				
				computeLF(perm, feats, label, ex, 0);
				
				assert(feats.size() > 0);
				initializePCA(feats, *pca, pcadim, pcadst);
				
				transformFeats(feats, *pca);
				cout << "nb of feats for learning gmm: " << feats.size() << endl;
				g->computeMixture(feats);
				
				if ( save_cache )
					g->saveData ( cache+"/gmmSIFT" );			
			}
			else
			{
				pca->read ( pcadst );
			}

			rfbovdyn->setPCA(pca);
			rfbovdyn->setGMM(g);
		}
		
		if(rfbovcrdf != NULL || rfCsurka != NULL)
		{
			Examples examples;
			VVector feats;
			vector<int> label;

			computeLF(perm, feats, label, examples , 1);

			FeaturePool fp;
			FeaturePool fpsparse;

			int dimension = examples[0].second.vec->size();
	
			for ( int i = 0 ; i < dimension ; i++ )
			{
				VectorFeature *f = new VectorFeature ( dimension, i );
				fp.addFeature(f, 1.0 / dimension);

				SparseVectorFeature *fs = new SparseVectorFeature ( dimension, i);
				//fs->feature_index = i;
				
				fpsparse.addFeature(fs, 1.0 / dimension);
			}

			if(rfbovcrdf != NULL)
			{
				RFBoVCodebook *rfbovdyn = dynamic_cast< RFBoVCodebook * >(rfbovcrdf);
				
				int maxDepth = conf->gI("BoVMoosmann", "maxdepth", 10);
				int csize = conf->gI("BoVMoosmann", "codebooksize", 1024);
				
				CodebookRandomForest *crdf = new CodebookRandomForest( maxDepth, csize );
				
				//RF anlernen
				FPCRandomForests *fpcrfmoos = new FPCRandomForests(conf, "MoosForest");

				fpcrfmoos->train(fp, examples);
					
				crdf->setClusterForest(fpcrfmoos);
			
				for(int i = 0; i < (int)examples.size(); i++)
				{
					if(examples[i].second.vec != NULL)
					{
						delete examples[i].second.vec;
						examples[i].second.vec = NULL;
					}
				}
				rfbovdyn->setCodebook(crdf);
			}

			if(rfCsurka != NULL)
			{

				bool usegmm = conf->gB("Csurka", "usegmm", false);
				bool usepca = conf->gB("Csurka", "usepca", false);
			
				PCA *pca = NULL;
				GMM *g = NULL;
				
				string classifierdst = cache+"/csurka.";
				
				if(usepca || usegmm)
				{

					RFCsurka *_rfcsurka = dynamic_cast< RFCsurka * >(rfCsurka);
				
					bool create = false;
					string gmmdst = cache+"/csurka.gmm";
					string pcadst = cache+"/csurka.pca";

					int pcadim = conf->gI("Csurka", "pcadim", 100);
				
					if(usepca)
					{
						pca = new PCA(pcadim);
					
						if (!FileMgt::fileExists(pcadst))
						{
							create = true;
						}
						else
						{
							pca->read ( pcadst );
						}
					}

					if(usegmm)
					{
						int gaussians = conf->gI("Csurka", "gaussians", 1024);
						g = new GMM( conf, gaussians );
					
						if ( !g->loadData ( gmmdst ) )
						{
							create = true;
						}				
					}

					if(create)
					{
						if(usepca)
						{
							convertExamplesToVVector(feats,examples, label);
							initializePCA(feats, *pca, pcadim, pcadst);
							transformFeats(feats, *pca);
							convertVVectorToExamples(feats,examples, label);
						}
						
						if(usegmm)
						{							
							g->computeMixture(examples);
							if ( save_cache )
								g->saveData ( gmmdst );	
						}
					}

				
					if(usepca)
						_rfcsurka->setPCA(pca);
				
					
					if(usegmm)
						_rfcsurka->setGMM(g);

				}
				
				
				string classifiertype = conf->gS("Csurka", "classifier", "SMLR");
				FeaturePoolClassifier *fpcrfCs = NULL;
				VecClassifier *vecClassifier = NULL;
				
				if(classifiertype == "SMLR")
				{
					fpcrfCs = new FPCSMLR(conf, "CsurkaSMLR");
					classifierdst += "smlr";
				}
				else if(classifiertype == "RF")
				{
					fpcrfCs = new FPCRandomForests(conf, "CsurkaForest");
					classifierdst += "rf";
				}
				else
				{
					vecClassifier = GenericClassifierSelection::selectVecClassifier(conf, classifiertype); 
					classifierdst += "other";
				}

				RFCsurka *rfcsurka = dynamic_cast< RFCsurka * >(rfCsurka);

				if(usepca)
				{
					assert(examples.size() > 0);
					if((int)examples[0].second.vec->size() != pca->getTargetDim())
					{
						for(int i = 0; i < (int)examples.size(); ++i)
						{
							*examples[i].second.vec = pca->getFeatureVector ( *examples[i].second.vec, true );
						}
					}
				}

								
				if ( !FileMgt::fileExists( classifierdst) )
				{
					if(usegmm)
					{
						if(classifiertype == "SMLR")
						{
							for(int i = 0; i < (int)examples.size(); ++i)
							{
								examples[i].second.svec = new SparseVector();
								g->getProbs(*examples[i].second.vec, *examples[i].second.svec);
								delete examples[i].second.vec;
								examples[i].second.vec = NULL;
							}
						}
						else
						{
							for(int i = 0; i < (int)examples.size(); ++i)
							{
								g->getProbs(*examples[i].second.vec, *examples[i].second.vec);
							}
						}
						if(fpcrfCs != NULL)
						{
						  fpcrfCs->train(fpsparse, examples);
						}
						else
						{
						  LabeledSetVector lvec;
						  convertExamplesToLSet(examples, lvec);
						  vecClassifier->teach(lvec);
						  convertLSetToExamples(examples, lvec);
						  vecClassifier->finishTeaching();
						}
					}
					else
					{
						if(fpcrfCs != NULL)
						{
						  fpcrfCs->train(fp, examples);
						}
						else
						{
						  LabeledSetVector lvec;
						  convertExamplesToLSet(examples, lvec);
						  vecClassifier->teach(lvec);
						  convertLSetToExamples(examples, lvec);
						  vecClassifier->finishTeaching();
						}
					}

					if(fpcrfCs != NULL)
					{
						fpcrfCs->setMaxClassNo(classNames->getMaxClassno());
						fpcrfCs->save ( classifierdst );
					}
					else
					{
						vecClassifier->setMaxClassNo(classNames->getMaxClassno());
						vecClassifier->save(classifierdst);
					}
	
				}
				else
				{
					if(fpcrfCs != NULL)
					{
						fpcrfCs->setMaxClassNo(classNames->getMaxClassno());
						fpcrfCs->read ( classifierdst );
					}
					else
					{
						vecClassifier->setMaxClassNo(classNames->getMaxClassno());
						vecClassifier->read(classifierdst);
					}

					
				}
	
				if(fpcrfCs != NULL)
				{
					rfcsurka->setClassifier(fpcrfCs);
				}
				else
				{
					rfcsurka->setClassifier(vecClassifier);
				}
			}
			fp.destroy();
			for(int i = 0; i < (int)examples.size(); i++)
			{
				if(examples[i].second.vec != NULL)
				{
					delete examples[i].second.vec;
					examples[i].second.vec = NULL;
				}
			}
		}
	}
	#ifdef DEBUG_PRINTS
	cerr << "SemSegRegionBased::learnHighLevel finished" << endl;
	#endif
}

void SemSegRegionBased::transformFeats(VVector &feats, PCA &pca)
{
	#ifdef DEBUG_PRINTS
	cerr << "SemSegRegionBased::transformFeats starts"<< endl;
	#endif
	for(int i = 0; i < (int)feats.size(); i++)
	{
		feats[i] = pca.getFeatureVector ( feats[i], true );
	}
	#ifdef DEBUG_PRINTS
	cerr << "SemSegRegionBased::transformFeats finished" << endl;
	#endif
}

void SemSegRegionBased::initializePCA ( const VVector &feats, PCA &pca, int dim, string &fn )
{
	#ifdef DEBUG_PRINTS
	cerr << "SemSegRegionBased::initializePCA starts"<< endl;
	#endif
	pca = PCA(dim);

	if (!FileMgt::fileExists(fn))
	{
		srand ( time ( NULL ) );

		int featsize = (int)feats.size();
		int maxfeatures = std::min ( dim*20, featsize );

		NICE::Matrix features ( maxfeatures, (int)feats[0].size() );

		for ( int i = 0; i < maxfeatures; i++ )
		{
			int k = rand() % featsize;

			int vsize = (int)feats[k].size();
			for(int j = 0; j < vsize; j++)
			{
				features(i,j) = feats[k][j];
			}
		}
		pca.calculateBasis ( features, dim, 0 );

		if ( save_cache )
			pca.save ( fn );

	}
	else
	{
		pca.read ( fn );
	}
	#ifdef DEBUG_PRINTS
	cerr << "SemSegRegionBased::initializePCA finished"<< endl;
	#endif
}