/* This file is part of the source code for the following publication:
 * Assembling Self-Supporting Structures, Deuss et al., SIGGRAPH Asia 2014
 *
 * Copyright (C) 2014 Mario Deuss <mario.deuss@epfl.ch>
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/. */


#include "SelfAssemblyPlugin.h"

// SelfAssembly
#include "Loader.h"
#include "Analyzer.h"
#include "EquilibriumForces.h"
#include "Statistics.h"
#include "Utils.h"
#include "AABB.h"
#include "Timer.h"

#include <iostream>
#include <fstream>
#include <iomanip>

#include <queue>

#include "QPMosek.h"
#include "QPFactory.h"


namespace SelfAssembly {

SelfAssemblyPlugin::SelfAssemblyPlugin()
	: assembly_(0),lastForces_(0),interfacesLoaded_(false)
		{ }

SelfAssemblyPlugin::~SelfAssemblyPlugin()
{
	if(assembly_){
		delete assembly_;
		assembly_=NULL;
	}
}


void SelfAssemblyPlugin::saveBlocksFile(SelfAssembly::Assembly& a,
										std::string* _fileName/*=NULL*/){
	
	std::string lastDir;
	bool b = SelfAssembly::Loader::writeBlocksToFile(a,lastDir,_fileName);
	MYOUT << __FUNCTION__ << ": Writing blocks " << (b?"":"un") << "successfull." << std::endl;
	if(!b){ return; }
}


void SelfAssemblyPlugin::staticAnalysis(const SelfAssembly::AnalyzerParameter& ap,
										bool segmentation,
										bool _debug /*=false*/ )
{
	if(!assembly_){
		MYOUT << __FUNCTION__ << ": No assembly created yet." << std::endl;
		return;
	}
	
	if(!interfacesLoaded_){
		MYOUT << __FUNCTION__ << ": No interfaces loaded yet." << std::endl;
		return;
	}
	
	assembly_->setDensity(ap.density);
	assembly_->setInterfaceScales(ap.interfaceScales);

	Timer t;
	t.start();
	
	QP* qp= QPFactory::make(static_cast<QPFactory::QPType>(ap.solverType));
	if(!qp){ MYOUT << __FUNCTION__ << ": Invalid Solver type. Aborting." << std::endl; delete qp; return;}
	QPMosek* msqp = dynamic_cast<QPMosek*>(qp);
	//msqp->setStoreVariables(true); //Dumping each optimization into a .task file
	
	SelfAssembly::Analyzer a(assembly_,*qp,ap,segmentation,_debug,1);
	a.setCheckRounding(true);
	
	bool newlF=false;
	if( lastForces_ == NULL){
		lastForces_ = new SelfAssembly::EquilibriumForces(assembly_);
		newlF=true;
	}
	bool b=a.staticEquilibrium(NULL,lastForces_);

	t.stop();
	if(!b){
		delete lastForces_;
		lastForces_ = NULL;
	}else{
		if(ap.report>0){
			MYOUT << __FUNCTION__ << ": took " << t.to_string() << std::endl;
			if(!a.lastRoundingCheckSuccessfull()){ MYOUT << ": Rounding check did not succeed." << std::endl;}
			MYOUT << Statistics::outputDeclaration() << " " << std::endl;
			MYOUT << "Internal force magnitude statistics " << std::endl;
			MYOUT << lastForces_->forceStatistics() <<std::endl;
			MYOUT << "Chain force magnitude statistics " << std::endl;
			MYOUT << lastForces_->chainStatistics() <<std::endl;
			MYOUT << "Interface force group norm statistics " << std::endl;
			MYOUT << lastForces_->intfsStatistics() <<std::endl;
		}
	}
	
	if(qp){
		delete qp;
	}
	
	return;
}

void SelfAssemblyPlugin::disabledNeighborsToQueue(SelfAssembly::Assembly& a,
												  BlockSet& nextBlocks,
												  bool _topDown,
												  const BlockSet* bs)
{
	for(unsigned int i=0;i<a.nB();++i){
		const SelfAssembly::B& b = a.b(i);
		if( (b.enabled()&&!_topDown) || (!b.enabled()&&_topDown) ){
			
			//add free neighboring blocks:
			for(unsigned int j=0;j<b.nIi();++j){
				const SelfAssembly::I& ij= a.i(b.ii(j));
				int bj=ij.bi(0);
				if(bj==static_cast<int>(i)){ bj=ij.bi(1); }
				if(bs!=NULL){
					if(bs->count(bj)==0){
						continue;
					}
				}
				if((!_topDown && !a.b(bj).enabled()) || (_topDown&&a.b(bj).enabled()&&!a.b(bj).fixed())){
					nextBlocks.insert(bj);
					a.addCluster(bj,b.cluster());
				}
			}
		}
	}
}

void SelfAssemblyPlugin::ifsPrioritiesToBlockset(const SelfAssembly::Assembly &a,
												 const IPriorities &ip,
												 BlockSets &bs,
												 const SelfAssembly::AnalyzerParameter& ap)
{
	double bbr = a.aabbRadius();
	std::vector<int> flag(a.nB(),-1);
	std::queue<int> q;
	
	unsigned int nState=0; //number of states
	int n=0; //current blockset #
	int cC=999999; //marks connected component in arch extraction
	int cnC=cC+1; //marks blocks visited but rejected during arch extraction
	int c=cC;//marker, differs from n only while n==0, during arch extraction

	do{
		bs.push_back(BlockSet());

		int ac=0;
		bool finishedArches=false;
		while(!finishedArches){

			finishedArches=true;
			if(n==0){ //initialize queue with first fixed and unvisited block
				for(unsigned int i=0;i<a.nB();++i){
					//if(ac>0){break;} //debug
					if(a.b(i).fixed() && flag.at(i)<0 ){
						q.push(i);
						finishedArches=false;
						break; }
				}
			}

			//flood-fil / connected component
			while(!q.empty()){
				int bni = q.front();
				q.pop();

				if(flag.at(bni)>=0){continue;}
				flag.at(bni)=c;
				const SelfAssembly::B& bn = a.b(bni);
				if(n>0){ bs.back().insert(bni); }

				//add free neighboring blocks to queue:
				for(unsigned int j=0;j<bn.nIi();++j){
					if(n==0 && ip.count(bn.ii(j))==0){continue;}
					const SelfAssembly::I& ij= a.i(bn.ii(j));
					if(n==0 && ij.fixed(a)){ continue; } //avoid flood between two fixed blocks
					int bj=ij.bi(0);
					if(bj==static_cast<int>(bni)){ bj=ij.bi(1); }
					if(flag.at(bj)==-1){
						q.push(bj);
					}
				}
			}

			if(n==0){ //Quasi-arch extraction

				bool doFilter=true;
				if( doFilter ){
					//filter if less than 2 boundary elements:
					AABB aabb; //bbx of centroids of fixed blocks
					unsigned int nb=0;
					for(unsigned int i=0;i<flag.size();++i){
						if( a.b(i).fixed() && flag.at(i)==cC ){
							aabb.extend(a.b(i).centroid());
							++nb;
						}
					}

					bool nbb = (nb>=2) || !ap.nFixedHeuristic;
					bool bbrb= (aabb.getRadius()>0.1*bbr) || !ap.radialHeuristic;

					if(nbb && bbrb){
						if(ap.singleHeuristic){
							//filter blocks flagged cC that have only a single cC neighbor
							bool removed=true;
							while(removed){
								removed=false;
								for(unsigned int i=0;i<flag.size();++i){
									if( flag.at(i)==cC && !a.b(i).fixed() ){
										SelfAssembly::Assembly::Neighbors N;
										a.addNeighbors(i,N);
										unsigned int ncC=0;
										for(auto it=N.begin(); it!=N.end();++it ){
											int bj = *it;
											if(flag.at(bj)==cC){
												++ncC;
											}
										}
										if(ncC<=1){
											flag.at(i)=cnC;
											removed=true;
											//break;
										}
									}
								}
							}
						}

						//convert cC-flags to n-flags
						for(unsigned int i=0;i<flag.size();++i){
							if( flag.at(i)==cC ){ flag.at(i)=n; }
						}

					}else{
						//convert all cC-flags to cnC-flags
						for(unsigned int i=0;i<flag.size();++i){
							if( flag.at(i)==cC ){ flag.at(i)=cnC; }
						}
					}
				}
				//convert cC-flags to n-flags
				for(unsigned int i=0;i<flag.size();++i){
					if( flag.at(i)==cC ){ flag.at(i)=n; }
				}
			}
		}
		++ac;

		if(n==0){

			if(ap.erosion){
				//filter blocks flagged cC that have a single component of neighbors
				bool removedC=true;
				while(removedC){
					removedC=false;
					for(unsigned int i=0;i<flag.size();++i){
						if( flag.at(i)==n && !a.b(i).fixed() ){
							SelfAssembly::Assembly::Neighbors N;
							a.addNeighbors(i,N,1);

							//flood-fill
							std::vector<int> nflag(a.nB(),-1);
							std::queue<int> nq;
							bool oneFree=false;
							for(auto it=N.begin(); it!=N.end();++it ){
								if(flag.at(*it)==n && nq.empty()){
									nq.push(*it);
								}
								if(flag.at(*it)!=n){
									oneFree=true;
								}
							}
							if(!oneFree){ continue; }

							while(!nq.empty()){
								int ni=nq.front();
								nq.pop();
								if(nflag.at(ni)>=0){ continue; }
								nflag.at(ni)=0;
								for(auto it=N.begin(); it!=N.end();++it ){
									int bj=*it;
									if(a.areNeighbors(ni,bj)){
										if(flag.at(bj)==n && nflag.at(bj)<0){
											nq.push(bj);
										}
									}
								}
							}

							//See if any neighbor not visited -> two components
							bool twoComp=false;
							for(auto it=N.begin(); it!=N.end();++it ){
								if(flag.at(*it)==n && nflag.at(*it)<0){
									twoComp=true; break;
								}
							}
							if(!twoComp){
								flag.at(i)=cnC;
								removedC=true;
							}
						}
					}
				}
			}

			//convert cnC-flags to -1 flags
			for(unsigned int i=0;i<flag.size();++i){
				if( flag.at(i)==cnC ){
					flag.at(i)=-1;
				}
				if( flag.at(i)==n){
					bs.back().insert(i);
				}
				//add unflaged fixed blocks to first blockset, flag with n-flag
				if( flag.at(i)==-1 && a.b(i).fixed() ){
					flag.at(i)=n;
					bs.back().insert(i);
				}
			}
		}
		n++;
		c=n;

		nState += bs.back().size();
		//MYOUT << "(state #" << nState << ")" <<  std::endl;

		//add first block with flag -1:
		for(unsigned int i=0;i<a.nB();++i){
			if(flag.at(i)==-1){ q.push(i); break; }
		}
	}while(std::find(flag.begin(),flag.end(),-1)!=flag.end());

	//sort by size:
	//the pre-++ excludes the arch-set
	std::sort(++(bs.begin()),bs.end(), [](const BlockSet& bsa, const BlockSet& bsb){ return bsa.size() > bsb.size(); });

	unsigned int totalN=0;
	//MYOUT << "[Blockset#,size]: ";
	for(unsigned int i=0;i<bs.size();++i){
		const BlockSet& s=bs.at(i);
		unsigned int n=s.size();
		totalN += n;
		//MYOUT << i << "," << n << " ";
	}
	//MYOUT << std::endl;
//	MYOUT << "Total number of blocks: " << totalN << std::endl;
	assert(totalN==a.nB());
}



int SelfAssemblyPlugin::sequenceAnalysis(const SelfAssembly::AnalyzerParameter &ap,
										 bool _removeUnusedChains,
										 bool _greedyHeuristic,
										 std::vector<int>* _seq,
										 const IPriorities*_priorities,
										 bool _topDown,
										 bool _debug,
										 bool _storeVariables)
{
	int _report = ap.report;

	if(_topDown){ MYERR << __FUNCTION__ << ": _topDown currently not supported." <<std::endl; }
	if(_seq!=NULL){ MYERR << __FUNCTION__ << ": _seq currently not supported." <<std::endl; }
	
	if(!assembly_){
		MYOUT << __FUNCTION__ << ": No assembly created yet." << std::endl;
		return -1;
	}
	
	if(!interfacesLoaded_){
		MYOUT << __FUNCTION__ << ": No interfaces loaded yet." << std::endl;
		return -1;
	}
	
	assembly_->setDensity(ap.density);
	assembly_->setInterfaceScales(ap.interfaceScales);
	
	if( lastForces_ != NULL){
		delete lastForces_;
		lastForces_=NULL;
	}
	lastForces_ = new SelfAssembly::EquilibriumForces(assembly_);
	lastForces_->fillNan();
	
	QP* qp= QPFactory::make(static_cast<QPFactory::QPType>(ap.solverType),_storeVariables);
	if(!qp){ MYOUT << __FUNCTION__ << ": Invalid Solver type. Aborting." << std::endl; delete qp; return -1;}
	
	Timer t;
	t.start();
	
	BlockSet nextBlocks;
	SelfAssembly::Sequence sequence;
	sequence.setNBFixed(assembly_->nB()-assembly_->nBFree());
	
	//disable blocks except for fixed ones first:
	for(unsigned int i=0;i<assembly_->nB();++i){
		assembly_->clearClusters();
		if(assembly_->b(i).fixed() || _topDown){
			assembly_->enableBlock(i);
			if(_seq==NULL && !_topDown){ sequence.addState(SelfAssembly::State(i));}
		}else{
			assembly_->disableBlock(i);
		}
	}
	
	//prepare the blockset clusters
	BlockSets bss(0);
	if(_priorities!=NULL){
		ifsPrioritiesToBlockset(*assembly_,*_priorities,bss,ap);

		//set blockset
		for(unsigned int i=0;i<bss.size();++i){
			const BlockSet& bs = bss.at(i);
			for(BlockSet::const_iterator bsi=bs.begin();bsi!=bs.end();++bsi){
					assembly_->setBlockSet(*bsi,i);
			}
		}
	}else{
		bss.push_back(BlockSet(assembly_->nB()));
		for(unsigned int i=0;i<assembly_->nB();++i){
			bss.at(0).insert(i);
		}
	}
	
	//initialization
	BlockSet candidates;

	SelfAssembly::EquilibriumForces lastF(assembly_);
	lastF.fillNan(); //to get correct chain change counting in first iteration
	SelfAssembly::Statistics sCa,sCr,sCt;
	SelfAssembly::StatisticsOfStatistics ssF,ssC,ssA;
	unsigned int count=0;
	//start main loop
	for(unsigned int bsIt=0;bsIt!=bss.size();++bsIt){

		//adding next blockset to candidates:
		BlockSet& bs = bss.at(bsIt);
		int bc=0;
		for(BlockSet::const_iterator cIt=candidates.begin();cIt!=candidates.end();++cIt){
			if(!assembly_->b(*cIt).fixed()) ++bc;
		}
		MYOUT << "Adding Blockset " << bsIt << " containing " << bs.size() << " elements. " << bc << " candidates left from previous sets." << std::endl;
		for(BlockSet::const_iterator bIt=bs.begin();bIt!=bs.end();++bIt){
			candidates.insert(*bIt);
		}

		//add unenabled neighboring blocks
		disabledNeighborsToQueue(*assembly_, nextBlocks,false,&candidates);
		
		while(!nextBlocks.empty()){

			bool anySolution=false;
			std::vector<double> stands(nextBlocks.size(),MYINF); //in case of greedy heuristics we need inf in the yet untried entries
			
			SelfAssembly::EquilibriumForces ef(assembly_);
			unsigned int i=0;
			for(BlockSet::iterator it=nextBlocks.begin();it!=nextBlocks.end();++it,++i){
				
				//try to add next block:
				int bni=*it;
				const SelfAssembly::B& bn = assembly_->b(bni);
				if(!_topDown){
					if(bn.enabled()){ assert(false); /*since we are using a set, this should never happen*/ continue; }
					assembly_->enableBlock(bni);
				}else{
					assembly_->enableBlock(bni); assert(false); //shouldnt that be disableBlock???
				}
				
				SelfAssembly::Analyzer a(assembly_,*qp,ap,false,_debug,0);
				a.setCheckRounding(true);
				bool b;
				if(!_greedyHeuristic | (count==0)){
					b=a.staticEquilibrium(NULL,&ef,&lastF);
				}else{
					b=a.staticEquilibrium(NULL,&ef,&lastF);
					if(b){
						if( (ef.chainStatistics().nnz) < (lastForces_->chainStatistics().nnz)){
							if(_topDown){
								assembly_->enableBlock(bni);
							}else{
								assembly_->disableBlock(bni);
							}
							stands.at(i)=-1.; //smaller than any other :-)
							if(_report>1){MYOUT << __FUNCTION__ << ": Greedy choice at " << i << " of " <<nextBlocks.size() <<std::endl;}
							anySolution = true;
							break;
						}
					}
				}
				anySolution |= b;
				
				if(b && a.lastRoundingCheckSuccessfull()){
					auto cc= ef.chainChange(lastF);
					stands.at(i) = (1.-ap.chainChangew) * ef.chainLpPowerP(ap.lp) + ap.chainChangew*(cc.first+cc.second);
					//stands.at(i) = ef.chainLpPowerP(ap.lp);//static_cast<double>(ef.chainStatistics().nnz);  //qp->functionValue();
					//if(ef.chainStatistics().nnz > 4 ){ stands.at(i)= MYINF; }
				}else{
					if(_report>1){
						if(!a.lastRoundingCheckSuccessfull()){
							MYERR << __FUNCTION__ << ": Rounding Check failed!" << std::endl;
						}
					}
					stands.at(i)=MYINF;
				}
				if(!_topDown){
					assembly_->disableBlock(bni);
				}else{
					assembly_->enableBlock(bni);
				}
				
				if(_report>1){MYOUT << *it << " " << std::setprecision(16) << stands.at(i) << std::endl;}
				
			}
			if(_report>1){MYOUT << std::endl;}
			
			if(!anySolution /*|| sequence.nStates()==6666*/){
				SelfAssembly::Analyzer a(assembly_,*qp,ap,false,_debug,1);
				bool b=a.staticEquilibrium(NULL,lastForces_,&lastF);
				MYOUT << __FUNCTION__ << ": No valid block found from " << nextBlocks.size() << " candidates." << std::endl;
				break;
			}else{
				
				//find min function value
				std::vector<double>::iterator minIt=std::min_element(stands.begin(),stands.end());
				BlockSet::iterator bit=nextBlocks.begin();
				std::vector<double>::iterator it;
				for(it=stands.begin();it!=minIt;++it,++bit){;}
				if(_report>1){MYOUT << __FUNCTION__ << ": *bit=" << *bit << ", obj: " << *it << ", sum: " << ssC.nnz.sum << std::endl;}
				
				//add corresponding block and resolve
				int bni=*bit;
				
				const SelfAssembly::B& bn = assembly_->b(bni);
				if(!_topDown){
					assert(!bn.enabled());
					assembly_->enableBlock(bni);
				}else{
					assert(bn.enabled());
					assembly_->disableBlock(bni);
				}
				assembly_->mergeClusters(bni);
				
				qp->setStoreVariables(true);
				SelfAssembly::Analyzer a(assembly_,*qp,ap,false,_debug,1);
				bool b=a.staticEquilibrium(NULL,lastForces_,&lastF);
				qp->setStoreVariables(false);
				//qp->setStorePath(); //reset

				if(_report>1){MYOUT << __FUNCTION__ << ": function value=" << std::setprecision(16) << qp->functionValue() <<std::endl;}
				//b=a.staticEquilibrium(NULL,lastForces_);
				if(!b){
					MYERR << __FUNCTION__ << ": No solution found for proposed block. Aborting. " <<std::endl;
					break;
				}
				SelfAssembly::State sni;
				lastForces_->state(bni,sni);
				sequence.addState(sni);

				
				//remove block from nextBlocks and candidates
				nextBlocks.erase(bni);
				candidates.erase(bni);
				
				//matlab_->eval("x_all=[x_all;x];");
				if(_removeUnusedChains){
					disableUnusedChains(lastForces_,assembly_);
				}
				
				if(_report>1){MYOUT << "||Force|| " << lastForces_->forceStatistics() << std::endl;
					MYOUT << "||Chain|| " << lastForces_->chainStatistics() << std::endl;}
				ssF.add(lastForces_->forceStatistics());
				ssC.add(lastForces_->chainStatistics());

				//MYOUT << count << ": AnchoStats " << lastForces_->anchorStatistics() << std::endl;
				//MYOUT << count << ": ChainStats " << lastForces_->chainStatistics() << std::endl;
				ssA.add(lastForces_->anchorStatistics());
				
				auto cC = lastForces_->chainChange(lastF);
				sCa.add(cC.first); sCr.add(cC.second);
				sCt.add(cC.first+cC.second);
				lastF = *lastForces_;
				


				//adding neighbors of new block bn to nextBlocks if in candidates
				for(unsigned int j=0;j<bn.nIi();++j){
					const SelfAssembly::I& ij= assembly_->i(bn.ii(j));
					int bj=ij.bi(0);
					if(bj==static_cast<int>(bni)){ bj=ij.bi(1); }
					const SelfAssembly::B& bc=assembly_->b(bj);
					if(candidates.count(bj)==0){ continue; }
					if(!bc.fixed()){
						if( (!_topDown && !bc.enabled()) || (_topDown && bc.enabled()) ){
							nextBlocks.insert(bj);
							assembly_->addCluster(bj,bn.cluster());
						}
					}
				}
			}
			++count;
		}
	}
	
	bool finished = (sequence.nStates()==assembly_->nB());

	lastSequence_ = sequence;
	std::stringstream ss;
	if(_report>0){
		ss << "Sequence: ";
		if(!finished){ ss << " (NOT COMPLETE) "; }
		for(unsigned int i=0;i<sequence.nStates();++i){
			ss << "," << sequence.state(i).addedBlock_;
		}
		ss <<std::endl;
		ss << "         " << Statistics::outputDeclaration() << std::endl;
		ss << "||Force||'s" << std::endl << ssF << std::endl;
		ss << "||Chain||'s" << std::endl << ssC << std::endl;
		ss << "#non-zero chains: " << ssC.nnz << std::endl;
		ss << "||Anchor sum's||" << std::endl << ssA << std::endl;
		ss << " Chains added  : " << sCa << std::endl;
		ss << " Chains removed: " << sCr << std::endl;
		ss << " Chain changes: " << sCt << std::endl;
	}

	t.stop();
	ss << __FUNCTION__ << " took " << t.to_string() << std::endl;
	MYOUT << ss.str();

	std::ofstream seqfile;
	std::string filename;
	if(!Loader::uniqueFilename(lastDir_+"/sequence",".txt",filename)){
		MYERR << __FUNCTION__<< ": No unique filename for sequence found. Will not store." << std::endl;
	}else{
		seqfile.open(filename);
		seqfile << sequence << std::endl << std::endl;
		seqfile << "File: " << assembly_->name() << std::endl;
		seqfile << "Priorities: " << std::endl;

		for( BlockSet::const_iterator bsI = priorities_.begin(); bsI!=priorities_.end(); ++bsI ){
			seqfile << "," << *bsI;
		}
		seqfile << std::endl << ap;
		seqfile << ss.str();
		seqfile.close();
	}
	
	//reenable all chains
	for(unsigned int ci=0;ci<assembly_->nC();++ci){
		assembly_->enableChain(ci);
	}

	if(qp){ delete qp; }

	if(!finished){
		return -1;
	}else{
		//if(_report>-1){ MYOUT << __FUNCTION__ << ": #Active chains: " << ssC.nnz << std::endl; }
		return sCt.sum;
	}
}

void SelfAssemblyPlugin::generateZOrderSequence( std::vector<int>& seq )
{
	if(!assembly_){
		MYOUT << __FUNCTION__ << ": No assembly created yet." << std::endl;
		return;
	}
	SelfAssembly::Assembly a=*assembly_;
	
	typedef std::pair<int,double> indz;
	std::vector<indz> zOrder;
	for(unsigned int i=0;i<a.nB();++i){
		zOrder.push_back(indz(i,a.b(i).centroid()(2)));
	}
	std::sort( zOrder.begin(),zOrder.end(),[](indz a, indz b){ return a.second < b.second; });
	
	seq = std::vector<int>(zOrder.size(),-1);
	for(unsigned int i=0; i<zOrder.size();++i){
		seq.at(i) = zOrder.at(i).first;
	}

}

void SelfAssemblyPlugin::generateFloodfillSequence( std::vector<int>& seq)
{
	if(!assembly_){
		MYOUT << __FUNCTION__ << ": No assembly created yet." << std::endl;
		return;
	}
	SelfAssembly::Assembly a=*assembly_;
	
	typedef std::pair<double,int> zind;
	class mycomp{public: bool operator()(const zind& a, const zind& b){return a.first>b.first;} };
	typedef std::priority_queue<zind,std::vector<zind>,mycomp> myq;
	myq nextBlocks;
	
	//disable blocks expect for fixed ones first:
	for(unsigned int i=0;i<assembly_->nB();++i){
		if(assembly_->b(i).fixed()){
			assembly_->enableBlock(i);
			seq.push_back(i);
		}else{
			assembly_->disableBlock(i);
		}
	}
	
	//add unenabled neighboring blocks
	for(unsigned int i=0;i<assembly_->nB();++i){
		const SelfAssembly::B& b = assembly_->b(i);
		if( b.enabled() ){
			
			//add free neighboring blocks:
			for(unsigned int j=0;j<b.nIi();++j){
				const SelfAssembly::I& ij= assembly_->i(b.ii(j));
				int bj=ij.bi(0);
				if(bj==static_cast<int>(i)){ bj=ij.bi(1); }
				if(!assembly_->b(bj).enabled()){
					nextBlocks.push(zind(assembly_->b(bj).centroid()(2), bj));
				}
			}
		}
	}
	
	while(!nextBlocks.empty()){
		
		int bni = (nextBlocks.top()).second;
		nextBlocks.pop();
		
		const SelfAssembly::B& bn = assembly_->b(bni);
		if(bn.enabled()){continue;}
		
		seq.push_back(bni);
		assembly_->enableBlock(bni);
		
		//add free neighboring blocks:
		for(unsigned int j=0;j<bn.nIi();++j){
			const SelfAssembly::I& ij= assembly_->i(bn.ii(j));
			int bj=ij.bi(0);
			if(bj==static_cast<int>(bni)){ bj=ij.bi(1); }
			if(!assembly_->b(bj).fixed() && !assembly_->b(bj).enabled()){
				nextBlocks.push(zind(assembly_->b(bj).centroid()(2), bj));
			}
		}
		
	}
	
}

void SelfAssemblyPlugin::computePriorities( const SelfAssembly::AnalyzerParameter& ap, IPriorities& ip )
{
	if(lastForces_==NULL){
		MYOUT << __FUNCTION__ << ": No forces computed yet. Aborting." << std::endl;
		return;
	}

	std::vector<int> p;
	ip.clear();
	for(unsigned int ii=0;ii<assembly_->nI();++ii){
		if(ap.coneMinimum <= lastForces_->archity(ii)){
			p.push_back(ii);
			ip.insert(ii);
		}
	}
}

void SelfAssemblyPlugin::enableBlocks(const std::vector<int> &blockIdxs)
{
	if(!assembly_){
		MYOUT << __FUNCTION__ << ": No assembly created yet." << std::endl;
		return;
	}
	
	if(!interfacesLoaded_){
		MYOUT << __FUNCTION__ << ": No interfaces loaded yet." << std::endl;
		return;
	}
	
	for( unsigned int bi=0;bi<assembly_->nB();++bi){
		assembly_->disableBlock(bi);
	}
	
	for( std::vector<int>::const_iterator it=blockIdxs.begin();it!=blockIdxs.end();++it){
		int bi = *it;
		if(bi<0 || bi> static_cast<int>(assembly_->nB())){
			MYERR << __FUNCTION__ << ": Ignoring invalid block index " << bi << std::endl;
			continue;
		}
		assembly_->enableBlock(bi);
	}
	
}


void SelfAssemblyPlugin::disableUnusedChains(const SelfAssembly::EquilibriumForces *_forces,
											 SelfAssembly::Assembly *_assembly)
{
	for(unsigned int ci=0;ci<_assembly->nC();++ci){
		if(_assembly->c(ci).enabled(*_assembly)){
			if(_forces->chainForceMagnitude(ci) < SelfAssembly::numericalZero()){
				_assembly->disableChain(ci);
			}
		}
	}
}


void SelfAssemblyPlugin::clearAssembly(){
	if(!assembly_){
		assembly_ = new SelfAssembly::Assembly();
	}else{
		assembly_->clear();
	}
	interfacesLoaded_=false;
	
	if(lastForces_){
		delete lastForces_;
		lastForces_ = NULL;
	}
	
}

void SelfAssemblyPlugin::loadBlocksFile( std::string* fileName/*=NULL*/, bool overwrite/*=false*/){
	if(!assembly_){
		assembly_ = new SelfAssembly::Assembly();
	}else{
		assembly_->clear();
	}
	interfacesLoaded_=false;
	
	std::string lastDir=lastDir_;
	bool b = SelfAssembly::Loader::loadBlocksFromFile(*assembly_, lastDir, fileName );
	if(!b){
		MYERR << __FUNCTION__ << ": Loading blocks unsuccessfull." << std::endl;
		delete assembly_; assembly_=NULL;
		return;
	}
	
	lastDir_=lastDir;
	double d=assembly_->density();
	//MYOUT << "Computed and set numerical rescale (density): " << d << std::endl;
	//MYOUT << "Numerical Zero: " << SelfAssembly::numericalZero() << std::endl;
	
	return;
}

void SelfAssemblyPlugin::loadInterfacesFile(std::string *fileName/*=NULL*/)
{
	if(!assembly_){
		assembly_ = new SelfAssembly::Assembly();
	}
	
	if(assembly_->nB()==0){
		MYERR << __FUNCTION__ << ": Assembly has no blocks yet. Please load blocks first!" << std::endl;
		return;
	}
	assembly_->clearInterfaces();
	
	std::string lastDir=lastDir_;
	bool b = SelfAssembly::Loader::loadInterfacesFromFile(*assembly_, lastDir, fileName );
	if(!b){ MYOUT << __FUNCTION__ << ": Loading interfaces unsuccessfull." << std::endl;}
	if(b){  lastDir_=lastDir;}
	interfacesLoaded_ = b;

	return;
}
}//namespaces\


