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

namespace SelfAssembly{

void EquilibriumForces::reset(const Assembly *_a){
	a_=_a;
	FInBasis_.resize(3*a_->totalVertices(a_->nI()),3);
	chainFMag_.resize(a_->nC(),1);
	upd_=false;
}

void EquilibriumForces::fillNan()
{
	FInBasis_.fill(MYNAN);
	chainFMag_.fill(MYNAN);
}

void EquilibriumForces::setForceInIfsBasis(Idx ifsi, Idx vi, const Force& f)
{
	FInBasis_.row(a_->totalVertices(ifsi)+vi) = f;
	upd_=false;
}


EquilibriumForces::Force EquilibriumForces::forceInIfsBasis( Idx ifsi, Idx vi) const
{	
	return FInBasis_.row(a_->totalVertices(ifsi)+vi);
}

EquilibriumForces::Force EquilibriumForces::force(Idx ifsi, Idx vi , bool scaled /*=false*/) const
{
	Force f = forceInIfsBasis(ifsi,vi);
	Force fR = a_->i(ifsi).basis()*f;
	if(scaled){
		if(forceStatistics().max > SelfAssembly::numericalZero()){
			fR /= forceStatistics().max;
		}
	}
	return fR;
}

bool EquilibriumForces::isActiveF(Idx ifsi, Idx vi) const
{
	return ( forceInIfsBasis(ifsi,vi).norm() > SelfAssembly::numericalZero());
}

double EquilibriumForces::cone(Idx ifsi) const
{
	if(!(a_->i(ifsi).enabled(*a_))){
		return MYNAN;
	}

	double n=0.;
	for( unsigned int iv=0;iv< a_->i(ifsi).nV();++iv){
		n += forceInIfsBasis(ifsi,iv).squaredNorm();
	}
	return std::sqrt(n);
}

double EquilibriumForces::anchorScalarSum(Idx ai) const
{
	double n=0;
	for(unsigned int ci=0;ci<a_->nC();++ci){
		if( isActiveC(ci) && a_->c(ci).ai()==ai ){
			n += chainForceMagnitude(ci);
		}
	}
	return n;
}

double EquilibriumForces::maxF(Idx ifsi) const
{
	const SelfAssembly::I& ifs=a_->i(ifsi);
	if(!(ifs.enabled(*a_))){
		return MYNAN;
	}

	double n=0.;
	for( unsigned int iv=0;iv< ifs.nV();++iv){
		n = std::fmax(n, forceInIfsBasis(ifsi,iv).norm());
	}
	return n;
}

double EquilibriumForces::maxFDotN(Idx ifsi) const
{
	const SelfAssembly::I& ifs=a_->i(ifsi);
	if(!(ifs.enabled(*a_))){
		return MYNAN;
	}

	double n=0.;
	for( unsigned int iv=0;iv< ifs.nV();++iv){
		n = std::fmax(n, std::fabs(forceInIfsBasis(ifsi,iv)(2)));
	}
	return n;
}

bool EquilibriumForces::isActiveI(Idx ifsi) const
{
	return ( cone(ifsi) > SelfAssembly::numericalZero() );
}

void EquilibriumForces::setChainForceMagnitude(Idx ci, double c)
{
	chainFMag_(ci)=c;
	upd_=false;
}

double EquilibriumForces::chainForceMagnitude(Idx ci,bool scaled /*=false*/) const
{
	double c=chainFMag_(ci);
	if(scaled){
		if( chainStatistics().max > SelfAssembly::numericalZero()){
			c /= chainStatistics().max;
		}
	}
	return c;
}

bool EquilibriumForces::isActiveC(Idx ci) const
{
	return ( chainForceMagnitude(ci) > SelfAssembly::numericalZero() ); //note the magnitue NaN also yields inactive.
}

double EquilibriumForces::chainLpPowerP(double p)
{
	double clp=0.0;
	for(unsigned int i=0;i<chainFMag_.size();++i){
		double c = chainFMag_(i);
		if(!std::isnan(c)){
			clp += std::pow(std::fabs(c),p);
		}
	}
	return clp;
}

std::pair<unsigned int, unsigned int> EquilibriumForces::chainChange(const EquilibriumForces &eqf)
{
	unsigned int nA,nR; nA=0; nR=0;
	for(unsigned int ci=0;ci<a_->nC();++ci){
		if( isActiveC(ci) && !eqf.isActiveC(ci)){ nA++; }
		if(!isActiveC(ci) &&  eqf.isActiveC(ci)){ nR++; }
	}
	return std::make_pair(nA,nR);
}

void EquilibriumForces::computeStatistics()
{
	if(upd_){ return; }
	upd_=true;

	fS_ = Statistics();
	for(unsigned int ii=0;ii<a_->nI();++ii){
		for(unsigned int ivi=0;ivi<a_->i(ii).nV();++ivi){
			double n=force(ii,ivi).norm();
			fS_.add(n);
		}
	}

	cS_=Statistics();
	for(unsigned int ci=0;ci<a_->nC();++ci){
		double c = chainForceMagnitude(ci);
		cS_.add(c);
	}

	iS_=Statistics();
	for(unsigned int ii=0;ii<a_->nI();++ii){
		if(a_->i(ii).enabled(*a_)){
			double c = cone(ii);
			iS_.add(c);
		}
	}

	aS_=Statistics();
	for(unsigned int ai=0;ai<a_->nA();++ai){
		double fa = anchorScalarSum(ai);
		aS_.add(fa);
	}

}

void EquilibriumForces::state(Idx bi, State &s) const
{
	s = State(bi);
	for(unsigned int ci=0;ci<a_->nC();++ci){
		if(isActiveC(ci)){
			s.activeChains_.push_back(State::ChainIdxs(a_->c(ci).ai(),a_->c(ci).bi()));
		}
	}
}

}
