/*!\file: convergence.cpp
 * \brief: figure out if convergence has been reached
 */ 

#include "../objects/objects.h"
#include "../modules/modules.h"
#include "../io/io.h"
#include "../EnumDefinitions/EnumDefinitions.h"

void convergence(bool* pconverged, Matrix* Kff,Vector* pf,Vector* uf,Vector* old_uf,Parameters* parameters){

	/*output*/
	bool converged=false;

	/*intermediary*/
	Vector* KU=NULL;
	Vector* KUF=NULL;
	Vector* KUold=NULL;
	Vector* KUoldF=NULL;
	Vector* duf=NULL;
	IssmDouble ndu,nduinf,nu;
	IssmDouble nKUF;
	IssmDouble nKUoldF;
	IssmDouble nF;
	IssmDouble solver_residue,res;

	/*convergence options*/
	IssmDouble eps_res;
	IssmDouble eps_rel;
	IssmDouble eps_abs;
	IssmDouble yts;

	/*If uf is NULL in input, f-set is nil, model is fully constrained, therefore converged from 
	 * the get go: */
	if(uf->IsEmpty()){
		*pconverged=true;
		return;
	}

	/*get convergence options*/
	parameters->FindParam(&eps_res,DiagnosticRestolEnum);
	parameters->FindParam(&eps_rel,DiagnosticReltolEnum);
	parameters->FindParam(&eps_abs,DiagnosticAbstolEnum);
	parameters->FindParam(&yts,ConstantsYtsEnum);

	/*Display solver caracteristics*/
	if (VerboseConvergence()){

		//compute KUF = KU - F = K*U - F
		KU=uf->Duplicate(); Kff->MatMult(uf,KU);
		KUF=KU->Duplicate(); KU->Copy(KUF); KUF->AYPX(pf,-1.0);

		//compute norm(KUF), norm(F) and residue
		nKUF=KUF->Norm(NORM_TWO);
		nF=pf->Norm(NORM_TWO);
		solver_residue=nKUF/nF;
		_pprintLine_("\n" << "   solver residue: norm(KU-F)/norm(F)=" << solver_residue);

		//clean up
		xdelete(&KU);
		xdelete(&KUF);
	}

	/*Force equilibrium (Mandatory)*/

	//compute K[n]U[n-1]F = K[n]U[n-1] - F
	KUold=uf->Duplicate(); Kff->MatMult(old_uf,KUold);
	KUoldF=KUold->Duplicate();KUold->Copy(KUoldF); KUoldF->AYPX(pf,-1.0);
	nKUoldF=KUoldF->Norm(NORM_TWO);
	nF=pf->Norm(NORM_TWO);
	res=nKUoldF/nF;
	if (xIsNan<IssmDouble>(res)){
		_pprintLine_("norm nf = " << nF << "f and norm kuold = " << nKUoldF << "f");
		_error2_("mechanical equilibrium convergence criterion is NaN!");
	}

	//clean up
	xdelete(&KUold);
	xdelete(&KUoldF);

	//print
	if(res<eps_res){
		if(VerboseConvergence()) _pprintLine_(setw(50) << left << "   mechanical equilibrium convergence criterion" << res*100 << " < " << eps_res*100 << " %");
		converged=true;
	}
	else{ 
		if(VerboseConvergence()) _pprintLine_(setw(50) << left << "   mechanical equilibrium convergence criterion" << res*100 << " > " << eps_res*100 << " %");
		converged=false;
	}

	/*Relative criterion (optional)*/
	if (!xIsNan<IssmDouble>(eps_rel) || (VerboseConvergence())){

		//compute norm(du)/norm(u)
		duf=old_uf->Duplicate(); old_uf->Copy(duf); duf->AYPX(uf,-1.0);
		ndu=duf->Norm(NORM_TWO); nu=old_uf->Norm(NORM_TWO);

		if (xIsNan<IssmDouble>(ndu) || xIsNan<IssmDouble>(nu)) _error2_("convergence criterion is NaN!");

		//clean up
		xdelete(&duf);

		//print
		if (!xIsNan<IssmDouble>(eps_rel)){
			if((ndu/nu)<eps_rel){
				if(VerboseConvergence()) _pprintLine_(setw(50) << left << "   Convergence criterion: norm(du)/norm(u)" << ndu/nu*100 << " < " << eps_rel*100 << " %");
			}
			else{ 
				if(VerboseConvergence()) _pprintLine_(setw(50) << left << "   Convergence criterion: norm(du)/norm(u)" << ndu/nu*100 << " > " << eps_rel*100 << " %");
				converged=false;
			}
		}
		else _pprintLine_(setw(50) << left << "   Convergence criterion: norm(du)/norm(u)" << ndu/nu*100 << " %");

	}

	/*Absolute criterion (Optional) = max(du)*/
	if (!xIsNan<IssmDouble>(eps_abs) || (VerboseConvergence())){

		//compute max(du)
		duf=old_uf->Duplicate(); old_uf->Copy(duf); duf->AYPX(uf,-1.0);
		ndu=duf->Norm(NORM_TWO); nduinf=duf->Norm(NORM_INF);
		if (xIsNan<IssmDouble>(ndu) || xIsNan<IssmDouble>(nu)) _error2_("convergence criterion is NaN!");

		//clean up
		xdelete(&duf);

		//print
		if (!xIsNan<IssmDouble>(eps_abs)){
			if ((nduinf*yts)<eps_abs){
				if(VerboseConvergence()) _pprintLine_(setw(50) << left << "   Convergence criterion: max(du)" << nduinf*yts << " < " << eps_abs << " m/yr");
			}
			else{
				if(VerboseConvergence()) _pprintLine_(setw(50) << left << "   Convergence criterion: max(du)" << nduinf*yts << " > " << eps_abs << " m/yr");
				converged=false;
			}
		}
		else  _pprintLine_(setw(50) << left << "   Convergence criterion: max(du)" << nduinf*yts << " m/yr");

	}

	/*assign output*/
	*pconverged=converged;
}
