/* 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 "Analyzer.h"
#include "Assembly.h"
#include "EquilibriumForces.h"

#include <Eigen/SparseCore>

#include <iomanip>      // std::setprecision

namespace SelfAssembly {

typedef Eigen::SparseMatrix<double> S;

Analyzer::Analyzer(const Assembly* _a ,
				   QP &_qp,
				   const AnalyzerParameter &ap,
				   bool segmentation /*=false*/,
				   bool _debug/*=false*/,
				   int _report /*=0*/):
	a_(_a),
	qp_(_qp),
	wBLsqTang_(a_->totalVertices(), ap.blockTangLsqw),
	wBLsqNorm_(a_->totalVertices(),ap.blockNormLsqw),
	wCLsq_(ap.chainLsqw),
	wCL1_(ap.chainL1w),
	wCL1s_(ap.chainL1Segw),
	wCcL1_(ap.chainChangew),
	wSeg_(a_->nI(),1.),
	irls_(ap.irls),
	interfaceGroupSparsity_(ap.interfaceGroupSparsity),
	checkRounding_(false),
	lastRoundingCheckSuccessfull_(false),
	friction_(ap.friction),
	anchorSafetyBound_(ap.anchorSafety),
	chainSafetyBound_(ap.chainSafety),
	segmentation_(segmentation),
	fixSelectedInterfacesToZero_(ap.fixSelectedInterfacesToZero),
	debug_(_debug),
	report_(_report),
	reweightChains_(ap.reweightL1Chains),
	reweightInterfaces_(ap.reweightL1Interfaces),
	minReweightIterations_(ap.minReweightIterations),
	maxReweightIterations_(ap.maxReweightIterations),
	lpReweight_(ap.lp){
	
	assert(ap.blockTangLsqw>=0.);
	assert(ap.blockNormLsqw>=0.);
	assert(wCLsq_>=0.);
	assert(wCL1_>=0.);
	assert(wCL1s_>=0.);
	assert(lpReweight_>0.); //Reweighting with p=0 does not make sense

	//initialize interface map:
	enabledIfsMap_.resize(a_->nI());
	unsigned int c=0;
	for(unsigned int ii=0;ii<a_->nI();++ii){
		if(a_->i(ii).enabled(*_a)){
			enabledIfsMap_.at(ii)=c;
			++c;
		}else{
			enabledIfsMap_.at(ii)=-1;
		}
	}
	nEI_=c;
	
	//initialize totalVertices count:
	totalVertices_.resize(nEI_);
	unsigned int t=0;
	unsigned int tt=0;
	for(unsigned int ii=0;ii<a_->nI();++ii){
		tt += a_->i(ii).nV();		
		int ei=enabledIfsMap_.at(ii);
		if(ei>=0){
			totalVertices_.at(ei)=t;
			t += a_->i(ii).nV();
		}
	}
	totalVertices_.push_back(t);
	
	
	//initialize block map:
	freeEnabledBlockMap_.resize(a_->nB());
	unsigned int fB=0;
	for(unsigned int bi=0;bi<a_->nB();++bi){
		if(a_->b(bi).fixed() || !a_->b(bi).enabled()){
			freeEnabledBlockMap_.at(bi)=-1;
		}else{
			freeEnabledBlockMap_.at(bi)=fB;
			++fB;
		}
	}
	nFEB_=fB;
	
	//initialized chain map:
	chainMap_.resize(a_->nC());
	unsigned int cc=0;
	for(unsigned int ci=0;ci<a_->nC();++ci){
		if(!a_->c(ci).enabled(*_a)){
			chainMap_.at(ci)=-1;
		}else{
			chainMap_.at(ci)=cc;
			++cc;
		}
	}
	nEC_=cc;
}

void Analyzer::xToForces(const VXd& x, EquilibriumForces* _resultF)
{
	int nTV=totalVertices_.back();
	_resultF->reset(a_);

	for(unsigned int ii=0;ii<a_->nI();++ii){
		int bI=enabledIfsMap_.at(ii);
		for(unsigned int ivi=0;ivi<a_->i(ii).nV();++ivi){

			Force f=vNan();
			if(bI>=0){
				int o = 3*(ivi+ totalVertices(bI));
				f = Force(x(o+0),x(o+1),x(o+2));
			}
			_resultF->setForceInIfsBasis(ii,ivi,f);
		}
	}

	unsigned int ofs=3*nTV;
	for(unsigned int ci=0;ci<a_->nC();++ci){
		int bC=chainMap_.at(ci);
		if(bC<0){
			_resultF->setChainForceMagnitude(ci,MYNAN);
		}else{
			double c=x(ofs+bC);
			_resultF->setChainForceMagnitude(ci,c);
		}
	}

	_resultF->computeStatistics();
}

void Analyzer::reweightChains(const VXd &x, const VXd &fr, VXd &f, Statistics* s /*=NULL*/)
{
	unsigned int ofs=3*totalVertices_.back();
	for(unsigned int ci=0;ci<a_->nC();++ci){
		int bC=chainMap_.at(ci);
		if(bC>=0){
			double xc = reweight(x(ofs+bC),lpReweight_);
			if(s!=NULL){ s->add(xc); }
			f(ofs+bC) = fr(ofs+bC)*xc;
		}
	}
}

void Analyzer::reweightInterfaces(const Analyzer::VXd &x, const VXd &fr, VXd &f, Statistics *s /*=NULL*/)
{
	unsigned int ofs = 3*totalVertices_.back()+nEC_;
	for(unsigned int ii=0;ii<a_->nI();++ii){
		int bI=enabledIfsMap_.at(ii);
		if(bI>=0){
			double xc = reweight(x(ofs+bI),lpReweight_);
			if(s!=NULL){ s->add(xc); }
			f(ofs+bI) = fr(ofs+bI)*xc;
		}
	}
}

double Analyzer::reweight(double v, double p){
	if(irls_){
		return 1./(std::pow(std::fabs(v),2.- p)+1.e-5);
	}else{
		return 1./(std::pow(std::fabs(v),1.- p)+1.e-5);
	}
	return MYNAN;
}

bool Analyzer::staticEquilibrium(const EquilibriumForces* _initF /*=NULL*/,
								 EquilibriumForces* _resultF /*=NULL*/,
								 const EquilibriumForces* _lastF /*=NULL*/)
{		
	Cones* cones=NULL;
	bool rescaleCones=false;
	bool frictionCones=false;
	if(!segmentation_ && !irls_){
		rescaleCones=true;
		frictionCones=true;
		cones = new Cones();
	}

	lastRoundingCheckSuccessfull_=false;
	
	//variables
	VXd x=VXd::Zero(nvars());
	int nTV=totalVertices_.back();

	VXd x0;
	if(_initF){ fillInitialForces(_initF, x0); }
	
	//Create sparse matrix for equality constraints
	Triplets tsC;
	VXd d;
	fillEquality(tsC,d);
	S C(6*nFEB_,x.size());
	C.setFromTriplets(tsC.begin(), tsC.end());
	
	//Create sparse matrix for inequality constraints
	Triplets ts2;
	unsigned int nA=a_->nA();
	if(anchorSafetyBound_==MYINF){nA=0;}
	fillInequality(ts2,frictionCones?cones:NULL);
	unsigned int nFC=4*nTV;
	if(frictionCones){ nFC=0.; }
	S A(nFC+nA,x.size());
	if(!ts2.empty()){
		A.setFromTriplets(ts2.begin(),ts2.end());
	}
	VXd b=VXd::Zero(nFC+nA);
	b.segment(nFC,nA)=VXd::Constant(nA,anchorSafetyBound_);
	
	//Create objective terms H and f
	Triplets tsH;
	VXd f;
	fillObjective( tsH,f,_lastF );
	S H(x.size(),x.size());
	if(!tsH.empty()){
		H.setFromTriplets(tsH.begin(),tsH.end());
	}

	if(a_->nC()==0 && tsH.empty() && !(segmentation_)){
		MYOUT << __FUNCTION__ << ": No chains and no lsq weight on the forces. The problem is ill-defined. Aborting." <<std::endl;
		return false;
	}
	
	//bounds:
	VXd lb;
	fillLowerBounds(lb);
	VXd ub;
	fillUpperBounds(ub);

	//cones
	if(segmentation_){
		if(cones==NULL){
			cones = new Cones();
		}else{
			cones->reserve(nEI_+cones->size());
		}
		for(unsigned int ii=0;ii<a_->nI();++ii){
			int ifs=enabledIfsMap_.at(ii);
			if(ifs>=0){
				Cone c;
				c.s=1.;
				c.idxs.push_back(3*nTV + nEC_ + ifs); //lhs

				unsigned int ofs=totalVertices_.at(ifs);
				for( unsigned int vi=0;vi<a_->i(ii).nV();++vi){
					for(unsigned int di=0;di<3;++di){
						if(interfaceGroupSparsity_ || di==2){
							c.idxs.push_back( 3*(ofs+vi) + di); //rhs
						}
					}
				}
				cones->push_back(c);
			}
		}
	}

	//Rescale problem to have normal cones:
	S Ds(0,0), Dsinv(0,0);
	if(cones!=NULL && rescaleCones){
		unsigned int nv=nvars();
		bool anyNotOne=false;
		VXd s=VXd::Constant(nvars(), 1, MYNAN );
		for(unsigned int ci=0;ci<cones->size();++ci){
			Cone& c = cones->at(ci);
			int idx = c.idxs.at(0);
			double sc = c.s;

			if(!std::isnan(s(idx)) && s(idx)!=sc){
				std::cerr << __FUNCTION__ << " Idx " << idx << " contained in multiple cones. Rescaling will be wrong. Aborting." <<std::endl;
				return false;
			}
			s(idx)=sc;
			if(sc!=1.){ anyNotOne=true; }
			c.s =1.; //set scale to 1. to avoid confusion in the QP class
		}

		Triplets dst; dst.reserve(nv);
		Triplets dsinvt; dsinvt.reserve(nv);
		for(unsigned int i=0;i<nv;++i){
			double si=s(i);
			if(std::isnan(si)){ si = 1.; }
			dst.push_back(Eigen::Triplet<double>(i,i,si));
			dsinvt.push_back(Eigen::Triplet<double>(i,i,1./si));
		}

		if(!anyNotOne){
			std::cerr << __FUNCTION__ << "rescaleCones set but no cone has weight different from 1.0. Ignoring." <<std::endl;
			rescaleCones=false;
		}else{
			Ds.resize(nv,nv);
			Ds.reserve(Eigen::VectorXi::Constant(nv,1));
			Ds.setFromTriplets(dst.begin(),dst.end());
			Dsinv.resize(nv,nv);
			Dsinv.reserve(Eigen::VectorXi::Constant(nv,1));
			Dsinv.setFromTriplets(dsinvt.begin(),dsinvt.end());
			//std::cout << Dsinv <<std::endl;

			f = f.transpose() * Ds;
			A = A*Dsinv;
			C = C*Dsinv;
			lb= Ds*lb;
			ub= Ds*ub;
		}
	}else{
		if(rescaleCones){ std::cerr << __FUNCTION__ << ": rescaleCones set but no cones found. Ignoring." <<std::endl; }
		rescaleCones=false;
	}
	
	/*
	//hack, test trivial solution:
	EquilibriumForces ef(a_);
	trivialSolution(ef);
	VXd xf;
	fillInitialForces(&ef,xf);
	MYOUT << __FUNCTION__ << "Trivial Solution: count(A*x>d)=" << ((A*xf).array() > b.array()).sum();
	MYOUT << ", ||C*x-d||=" << (C*xf-d).norm() << ", count(x<lb)=" << (xf.array()<lb.array()).sum();
	MYOUT << ", count(x>ub)=" << (xf.array()>ub.array()).sum() <<std::endl;
	//endhack
	*/

	bool s=false;
	int diff=0;
	Statistics sc,si;
	double lpC, lpI, lpIC, lastlpC, lastlpI, lastlpIC;
	lpC=MYINF; lpI=MYINF; lpIC=MYINF;
	int nxc,nxcl,nxi,nxil;
	nxc=0;nxi=0;
	bool decrease=false;
	int i=0;
	VXd xOld;
	//MYOUT << "p" << lpReweight_ << "=[" << std::endl;
	VXd fr=f;
	VXd fEmpty(0,1);
	do{
		xOld=x;

		//rescale objective if cone rescale is taken care of in using variable fr for reweighting
		if(irls_){
			Triplets ht; ht.reserve(f.size());
			for(unsigned int i=0;i<f.size();++i){
				ht.push_back(Eigen::Triplet<double>(i,i,2.*f(i)));
			}
			H.setFromTriplets(ht.begin(),ht.end());
		}

		if(_initF==NULL){
			s=qp_.solve(H,irls_?fEmpty:f,A,b,C,d,lb,ub,x,NULL,cones,debug_);
		}else{
			s=qp_.solve(H,irls_?fEmpty:f,A,b,C,d,lb,ub,x,&x0,cones,debug_);
		}

		if( (report_==1 && !s) || (report_>1) ){MYOUT << __FUNCTION__ << ": " << qp_ << std::endl;}

		if(s && _resultF!=NULL){
			if(rescaleCones){
				VXd dx=Dsinv*x;
				xToForces(dx, _resultF);
			}else{
				xToForces(x, _resultF);
			}
		}

		if(!reweightChains_ && !reweightInterfaces_){ break; }

		//MYOUT << "f" << i << "=[" << f << "];" << std::endl;
		nxcl=sc.nnz; nxil=si.nnz; diff=0;
		sc=Statistics(); si=Statistics();
		lastlpIC=lpIC; lpIC=0.;
		if(reweightChains_){
			reweightChains(x,fr,f,&sc);
			diff += std::abs((int)(sc.nnz - nxcl));
			lastlpC=lpC;
			lpC = lpNormPowP(x.segment(3*nTV,nEC_),lpReweight_);
			lpIC += lpC;
		}
		if(segmentation_ && reweightInterfaces_){
			reweightInterfaces(x,fr,f,&si);
			diff += std::abs((int)(si.nnz-nxil));
			lastlpI=lpI;
			lpI = lpNormPowP(x.segment(3*nTV+nEC_,nEI_),lpReweight_);
			lpIC += lpI;
		}
		decrease = (lpIC < lastlpIC);
		if(!decrease){x=xOld;}
		//MYOUT << i <<"," << std::setprecision(16) << lpC << "," << lpI << ";" << std::endl;
		++i;
	}while( i<maxReweightIterations_ && (i<minReweightIterations_ || decrease ) );
	//MYOUT << "];" << std::endl;

	if(rescaleCones){ x = Dsinv*x; }

	if(s && checkRounding()){

		if(cones!=NULL){
			//check if scaled cones are satisfied:
			for(unsigned int ci=0;ci<cones->size();++ci){
				Cone& c = cones->at(ci);
				double xc0 = x(c.idxs.at(0));
				double lhs = c.s * xc0;
				double rhs = 0.;
				for(unsigned int cci=1;cci<c.idxs.size();++cci){
					rhs += std::pow(x(c.idxs.at(cci)),2.);
				}
				rhs = std::sqrt(rhs);
				if(lhs < rhs){
					MYERR << __FUNCTION__ << ": Scaled cone constraint not satisfied by " << rhs-lhs << std::endl;
				}
			}
		}

		//fix numerical zeros to 0.0
		Triplets tsCr;
		VXd dr;
		fillEquality(tsCr,dr);
		std::vector<unsigned int> zeros;
		unsigned int cr=0;
		double maxR = 0.0;
		for(unsigned int i=3*nTV;i<3*nTV+nEC_;++i){
			bool isZero = ( std::fabs(x(i)) < numericalZero() );
			if( isZero ){
				tsCr.push_back(Triplet(6*nFEB_+cr,i,1.0));
				maxR = std::fmax(std::fabs(x(i)), maxR);
				++cr;
				zeros.push_back(i);
			}
		}

		S Cr(6*nFEB_ + cr,3*nTV + nEC_ );
		Cr.setFromTriplets(tsCr.begin(), tsCr.end());

		dr.conservativeResize(6*nFEB_ + cr);
		dr.segment(6*nFEB_,cr) = VXd::Zero(cr);


		//see if a solution is found:
		VXd xR=x;
		bool bc=qp_.solve(H,f,A,b,Cr,dr,lb,ub,xR,NULL,cones,debug_);

		if(!bc){
			MYERR << __FUNCTION__ << ": rounding check: failed to find solution after rounding and fixing to zero." <<std::endl;
			lastRoundingCheckSuccessfull_=false;
		}else{
			//check if zeros are indeed 0.0
			double maxAbs=0.;
			for(unsigned int z=0;z<zeros.size();++z){
				unsigned int i = zeros.at(z);
				double xri=	xR(i);
				double xria=std::fabs(xR(i));
				maxAbs = std::fmax(xria, maxAbs);
				if(xria != 0.0){
					MYOUT << __FUNCTION__ << ": rounding check: variable " << i << " not exactly zero but << " <<  xri <<std::endl;
				}
			}

			if(maxAbs != 0.0){
				MYERR << __FUNCTION__ << ": rounding check: variables not exactly zero. |max|=" << maxAbs << ",before rounding |maxR|=" << maxR <<std::endl;
				lastRoundingCheckSuccessfull_=false;
			}else{
				lastRoundingCheckSuccessfull_=true;
			}
		}
	}

	if(cones!=NULL){ delete cones; cones=NULL; }
	
	//return solver success
	return s;
}

void Analyzer::fillInitialForces( const EquilibriumForces* _initF, VXd& x0){
	
	//check if properly initialized:
	assert(_initF);
	assert(_initF->a() == this->a_);
	
	int nTV=totalVertices_.back();
	x0.resize(nvars());
	
	//Fill initial interface vertex forces from initF if existent
	for( unsigned int ii=0;ii<a_->nI();++ii){
		int iF=enabledIfsMap_.at(ii);
		if(iF<0){continue;}
		const I& ifs=a_->i(ii);
		assert(ifs.enabled(*a_));
		
		int offs = 3*totalVertices(iF);
		for( unsigned int vi=0;vi<ifs.nV();++vi){
			Force f=_initF->forceInIfsBasis(ii,vi);
			if( std::isnan(f.squaredNorm())){ f=Force::Constant(0.); }
			x0.segment<3>(offs+3*vi) = f;
		}
	}

	//Fill initial chain force magnitude from initF if existent
	for( unsigned int ci=0;ci<a_->nC();++ci){
		int fI = chainMap_.at(ci);
		if(fI<0){ continue; }

		double f=_initF->chainForceMagnitude(ci);
		if( std::isnan(f) ){ f=0.;}
		x0(3*nTV + fI) = f;
	}
}

void Analyzer::fillEquality(Triplets& tsC, VXd& d){
	
	int nTV=totalVertices_.back();
			
	//Add equilibrium contribution from interfaces
	for( unsigned int ii=0;ii<a_->nI();++ii){
		int iF=enabledIfsMap_.at(ii);
		if(iF<0){continue;}
		const I& ifs=a_->i(ii);
		assert(ifs.enabled(*a_));
		
		for(unsigned int d=0;d<2;++d){
			unsigned int bi=ifs.bi(d);
			int bC=freeEnabledBlockMap_.at(bi);
			if(bC>=0){
				const B& b=a_->b(bi);
				Matrix6Xd m;
				blockInterfaceMatrix(m,b,ifs);
				if(d==0){ m = -m; }	//Interface basis points towards b1. For b0 to have compressive forces,
				//we need to flip the basis, which is the same as negating m
				fillBlock(m,tsC,6*bC,3*totalVertices(iF));
			}
		}
	}

	//Add equilibrium contribution from chains
	for( unsigned int ci=0;ci<a_->nC();++ci){
		int fI = chainMap_.at(ci);
		if(fI<0){ continue; }
		
		const C& c=a_->c(ci); assert(c.enabled(*a_));
		int bC=freeEnabledBlockMap_.at(c.bi()); 
		assert(bC>=0);
		
		const B& b=a_->b(c.bi());
		const A& a=a_->a(c.ai());
		Vector6d v;
		blockChainMatrix(v,b,a);
		fillBlock(v,tsC,6*bC,3*nTV + fI);
	}
	
	//Fill rhs
	fillEqRhs(d);
}


unsigned int Analyzer::totalVertices( Idx ifsi ) const
{
	return totalVertices_.at(ifsi);
}

void Analyzer::fillBlock(const Eigen::MatrixXd& m, Triplets& ts, Idx row, Idx col ){
	
	for(int r=0;r<m.rows();++r){
		for(int c=0;c<m.cols();++c){
			if(m(r,c)!=0.){
				ts.push_back( Triplet(r+row,c+col,m(r,c)));
			}
		}
	}
}

void Analyzer::blockInterfaceMatrix( Matrix6Xd& m, const B& b, const I& ifs )
{
	unsigned int nV=ifs.nV();
	const Eigen::Matrix3d& f=ifs.basis();
	const V& c=b.centroid();	

	m.resize(6,3*nV);
	
	for(unsigned int j=0;j<nV;++j){
		m.block<3,3>(0,3*j) = f;
		
		const V& vj = ifs.v(j);
		Eigen::Matrix3d T;
		for(int d=0;d<3;++d){ T.col(d)= f.col(d).cross(vj-c); }
		m.block<3,3>(3,3*j) = T;
	}
}

void Analyzer::blockChainMatrix( Vector6d& v, const B& b, const A& a ){

	assert(b.hasHook());
	const V& h=b.hook();
	const V& c=b.centroid();
	V r=(a-h).normalized();

	v.block<3,1>(0,0) = r;
	v.block<3,1>(3,0) = r.cross(h-c);
}

void Analyzer::fillEqRhs(VXd &d)
{
	d.resize(6*nFEB_);
	
	for( unsigned int bi=0;bi<a_->nB();++bi){
		int bC=freeEnabledBlockMap_.at(bi);
		if(bC<0){ continue;}
		const B& b=a_->b(bi);
		
		//Negativ because its on the rhs
		V fext = -(b.volume()*a_->density())*gravity();
		d(6*bC+0) = fext(0);
		d(6*bC+1) = fext(1);
		d(6*bC+2) = fext(2);
		d(6*bC+3) = 0;
		d(6*bC+4) = 0;
		d(6*bC+5) = 0;
	}
}

void Analyzer::fillInequality( Triplets& ts, Cones* cones/*=NULL*/ ){
	unsigned int ofsr=0;
	unsigned int ofsc=0;
	for(unsigned int i=0;i<a_->nI();++i){
		int bI=enabledIfsMap_.at(i);
		if(bI<0){continue;}
		
		int ri=4*totalVertices(bI);
		int ci=3*totalVertices(bI);
		const I& ifs= a_->i(i);
		
		for( unsigned int v=0;v<ifs.nV();++v){
			if(cones==NULL){
				//conservative friction cone
				double f=friction_/std::sqrt(2.);
				Eigen::Matrix<double,4,3> m;
				m << 1.,0.,-f, -1.,0.,-f, 0.,1.,-f, 0.,-1.,-f;
				fillBlock(m,ts,ofsr+ri+4*v,ofsc+ci+3*v);
			}else{
				//friction cone:
				Cone c;
				c.idxs.push_back(ci+3*v+2);
				c.idxs.push_back(ci+3*v+0);
				c.idxs.push_back(ci+3*v+1);
				c.s = friction_;
				cones->push_back(c);
			}
		}

	}

	int nTV=totalVertices_.back();
	ofsr=4*nTV;
	ofsc=3*nTV;
	if(cones!=NULL){ ofsr=0; /*ofsc=0;*/ }

	if(anchorSafetyBound_<MYINF){
		for(unsigned int ai=0;ai<a_->nA();++ai){
			for( unsigned int ci=0;ci<a_->nC();++ci){
				const C& c=a_->c(ci);
				if(c.ai()==static_cast<int>(ai)){
					if(c.enabled(*a_)){
						int cc=chainMap_.at(ci);
						assert(cc>=0);
						ts.push_back(Triplet(ofsr+ai,ofsc+cc,1.0));
					}
				}
			}
		}
	}
}

void Analyzer::fillLowerBounds( VXd &lb){
	unsigned int n=totalVertices_.back();
	lb.resize(nvars());
	
	for(unsigned int i=0;i<n;++i){
		for( unsigned int o=0;o<3;++o){
			if(o==2){
				lb(3*i+o) = 0.;
			}else{
				lb(3*i+o) = -MYINF;
			}
		}
	}
	unsigned int ofs=3*n;
	
	for(unsigned int i=0;i<nEC_;++i){
		lb(ofs + i) = 0.;
	}
	ofs = 3*n+nEC_;

	//segmentation sparsity variables (1 per interface)
	if(segmentation_){
		for(unsigned int ii=0;ii<a_->nI();++ii){
			int bI=enabledIfsMap_.at(ii);
			if(bI<0){ continue; }

			double d = -MYINF;
			if(a_->i(ii).selected()&&fixSelectedInterfacesToZero_){d = 0.;}
			lb(ofs+bI) = d;
		}
	}
}

void Analyzer::fillUpperBounds( VXd &ub){
	ub.resize(nvars());
	ub.fill(MYINF);

	unsigned int n=totalVertices_.back();
	ub.resize(nvars());

	//interface forces are unbounded
	unsigned int ofs=3*n;

	//bounds on chains
	for(unsigned int i=0;i<nEC_;++i){
		ub(ofs + i) = chainSafetyBound_;
	}
	
	//segmentation sparsity variables (1 per interface)
	if(segmentation_){
		unsigned int ofs = 3*totalVertices_.back()+nEC_;
		for(unsigned int ii=0;ii<a_->nI();++ii){
			int bI=enabledIfsMap_.at(ii);
			if(bI<0){ continue; }

			double d = MYINF;
			if(a_->i(ii).selected()&&fixSelectedInterfacesToZero_){ d = 0.;}
			ub(ofs+bI) = d;
		}
	}
}

void Analyzer::fillObjective( Triplets& tsH, VXd& f, const EquilibriumForces* lastForces ){
	unsigned int n=totalVertices_.back();
	f=VXd::Zero(nvars());
	
	//interblock forces
	unsigned int c=0;
	for( unsigned int ii=0;ii<a_->nI();++ii){
		int bI=enabledIfsMap_.at(ii);
		if(bI<0){ continue; }

		const I& ifs=a_->i(ii);
		for(unsigned int j=0;j<ifs.nV();++j){
			W wt=wBLsqTang(a_->totalVertices(ii)+j);
			W wn=wBLsqNorm(a_->totalVertices(ii)+j);
			if(wt > 0. || wn > 0.){
				double m = static_cast<double>(ifs.m(j));
				tsH.push_back(Triplet(3*c+0,3*c+0,m*wt));
				tsH.push_back(Triplet(3*c+1,3*c+1,m*wt));
				tsH.push_back(Triplet(3*c+2,3*c+2,m*wn));
			}
			++c;
		}
	}
	assert(c==n);

	unsigned int ofs=3*n;
	
	//chain forces
	if( wCL1() > 0. ){
		for(unsigned int ci=0;ci<a_->nC();++ci){
			int bC=chainMap_.at(ci);
			if(bC<0){ continue; }
			double w=1.;
			if(!segmentation_){
				w = (1.-wCcL1());
				if(lastForces!=NULL){
					if(!lastForces->isActiveC(ci)){ 
						w+=wCcL1(); 
					}
				}
			}
			f(ofs+bC) = wCL1()*w;
		}
	}
	if( wCLsq_ > 0. ){
		for(unsigned int ci=0;ci<a_->nC();++ci){
			int bC=chainMap_.at(ci);
			if(bC<0){ continue; }
			tsH.push_back(Triplet(ofs+bC,ofs+bC,wCLsq_));
		}
	}
	ofs = 3*n+nEC_;

	//segmentation sparsity variables (1 per interface)
	if(segmentation_){
		for(unsigned int ii=0;ii<a_->nI();++ii){
			int i=enabledIfsMap_.at(ii);
			if(i>=0){
				f(ofs+i) = wSeg(ii);
			}
		}
	}
}

void Analyzer::trivialSolution( EquilibriumForces & f)
{
	const Assembly& a=*f.a();
	
	//all inter-block forces are zero
	for(unsigned int ii=0;ii<a.nI();++ii){
		for(unsigned int ivi=0;ivi<a.i(ii).nV();++ivi){
			V z = V::Zero();
			f.setForceInIfsBasis(ii,ivi,z);
		}
	}
	
	for(unsigned int ib=0;ib<a.nB();++ib){
		const B& b=a.b(ib);
		if(b.fixed()){continue;}
		
		//check that hook is above centroid:
		double nz = numericalZero();
		V hc = b.hook() - b.centroid();
		if( std::fabs(hc(0)) > nz || std::fabs(hc(1)) > nz ){
			std::cerr << __FUNCTION__ << ": hook not above centroid in block[" << ib << "]." << std::endl;
		}
		
		//distribute external forces fe to chains
		V fe = -b.volume()*a.density()*gravity();
		Eigen::Matrix3Xd M(3,b.nCi());
		for(unsigned int iic=0; iic<b.nCi();++iic){
			const C& c=a.c(b.ci(iic));
			M.col(iic) = (a.a(c.ai()) - b.hook()).normalized();
		}
		//Solve  M*w=fe in the lsq sense
		VXd w=M.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(fe);
		for(unsigned int iic=0; iic<b.nCi();++iic){
			Idx ic = b.ci(iic);
			f.setChainForceMagnitude(ic,w(iic));
			if(w(iic) < 0.){ std::cerr << __FUNCTION__ << ": chain magnitude (" << w(iic) << ") less than zero in block[" << ib << "]." << std::endl;
				std::cerr << "M: " << M << std::endl << "w: " <<w<<std::endl;
			}
		}
	}
	
}


double Analyzer::wBLsqNorm( Idx iv ) const
{
	return wBLsqNorm_.at(iv);
}

void Analyzer::setWBLsqNorm(double w)
{
	wBLsqNorm_ = Ws(a_->totalVertices(),w);
}

void Analyzer::setWBLsqNorm(Analyzer::Idx iv, double w)
{
	wBLsqNorm_.at(iv) = w;
}

double Analyzer::wSeg(Analyzer::Idx ii) const
{
	return wSeg_.at(ii);
}

void Analyzer::setWSeg(Analyzer::Idx ii, double w)
{
	wSeg_.at(ii) = w;
}

bool Analyzer::checkRounding() const
{
	return  checkRounding_;
}

void Analyzer::setCheckRounding(bool b)
{
	checkRounding_=b;
}

double Analyzer::wBLsqTang(Idx iv ) const
{
	return wBLsqTang_.at(iv);
}

void Analyzer::setWBLsqTang(double w)
{
	wBLsqTang_ = Ws(a_->totalVertices(),w);
}

void Analyzer::setWBLsqTang(Analyzer::Idx iv, double w)
{
	wBLsqTang_.at(iv) = w;
}

double Analyzer::wCL1() const
{
	if(segmentation_){
		return wCL1s_;
	}
	return wCL1_;
}

double Analyzer::wCLsq() const
{
	return wCLsq_;
}

void Analyzer::setWCLsq(double wCLsq)
{
	wCLsq_ = wCLsq;
}

}
