/*!\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, Mat Kff,Vec pf,Vec uf,Vec old_uf,Parameters* parameters){

	/*output*/
	bool converged=false;

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

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

	/*If uf is NULL in input, f-set is nil, model is fully constrained, therefore converged from 
	 * the get go: */
	if(!uf){
		*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
		VecDuplicate(uf,&KU); MatMultPatch(Kff,uf,KU);
		VecDuplicate(KU,&KUF);VecCopy(KU,KUF); VecAYPX(KUF,-1.0,pf);

		//compute norm(KUF), norm(F) and residue
		VecNorm(KUF,NORM_2,&nKUF);
		VecNorm(pf,NORM_2,&nF);
		solver_residue=nKUF/nF;
		_printf_(true,"\n%s%g\n","   solver residue: norm(KU-F)/norm(F)=",solver_residue);

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

	/*Force equilibrium (Mandatory)*/

	//compute K[n]U[n-1]F = K[n]U[n-1] - F
	VecDuplicate(uf,&KUold); MatMultPatch(Kff,old_uf,KUold);
	VecDuplicate(KUold,&KUoldF);VecCopy(KUold,KUoldF); VecAYPX(KUoldF,-1.0,pf);
	VecNorm(KUoldF,NORM_2,&nKUoldF);
	VecNorm(pf,NORM_2,&nF);
	res=nKUoldF/nF;
	if (isnan(res)){
		_printf_(true,"norm nf = %lf and norm kuold = %lf\n",nF,nKUoldF);
		_error_("mechanical equilibrium convergence criterion is NaN!");
	}

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

	//print
	if(res<eps_res){
		_printf_(VerboseConvergence(),"%-50s%g%s%g%s\n","   mechanical equilibrium convergence criterion",res*100," < ",eps_res*100," %");
		converged=true;
	}
	else{ 
		_printf_(VerboseConvergence(),"%-50s%g%s%g%s\n","   mechanical equilibrium convergence criterion",res*100," > ",eps_res*100," %");
		converged=false;
	}

	/*Relative criterion (optional)*/
	if (!isnan(eps_rel) || (VerboseConvergence())){

		//compute norm(du)/norm(u)
		VecDuplicate(old_uf,&duf);VecCopy(old_uf,duf); VecAYPX(duf,-1.0,uf);
		VecNorm(duf,NORM_2,&ndu); VecNorm(old_uf,NORM_2,&nu);

		if (isnan(ndu) || isnan(nu)) _error_("convergence criterion is NaN!");

		//clean up
		VecFree(&duf);

		//print
		if (!isnan(eps_rel)){
			if((ndu/nu)<eps_rel){
				_printf_(VerboseConvergence(),"%-50s%g%s%g%s\n","   Convergence criterion: norm(du)/norm(u)",ndu/nu*100," < ",eps_rel*100," %");
			}
			else{ 
				_printf_(VerboseConvergence(),"%-50s%g%s%g%s\n","   Convergence criterion: norm(du)/norm(u)",ndu/nu*100," > ",eps_rel*100," %");
				converged=false;
			}
		}
		else _printf_(true,"%-50s%g%s\n","   Convergence criterion: norm(du)/norm(u)",ndu/nu*100," %");

	}

	/*Absolute criterion (Optional) = max(du)*/
	if (!isnan(eps_abs) || (VerboseConvergence())){

		//compute max(du)
		VecDuplicate(old_uf,&duf);VecCopy(old_uf,duf); VecAYPX(duf,-1.0,uf);
		VecNorm(duf,NORM_2,&ndu); VecNorm(duf,NORM_INFINITY,&nduinf);
		if (isnan(ndu) || isnan(nu)) _error_("convergence criterion is NaN!");

		//clean up
		VecFree(&duf);

		//print
		if (!isnan(eps_abs)){
			if ((nduinf*yts)<eps_abs){
				_printf_(VerboseConvergence(),"%-50s%g%s%g%s\n","   Convergence criterion: max(du)",nduinf*yts," < ",eps_abs," m/yr");
			}
			else{
				_printf_(VerboseConvergence(),"%-50s%g%s%g%s\n","   Convergence criterion: max(du)",nduinf*yts," > ",eps_abs," m/yr");
				converged=false;
			}
		}
		else  _printf_(true,"%-50s%g%s\n","   Convergence criterion: max(du)",nduinf*yts," m/yr");

	}

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