/*!\file: diagnostic_core_nonlinear.cpp
 * \brief: core of the diagnostic solution for non linear materials
 */ 

#undef __FUNCT__ 
#define __FUNCT__ "cielodiagnostic_core_nonlinear"

#include "../toolkits/toolkits.h"
#include "../objects/objects.h"
#include "../EnumDefinitions/EnumDefinitions.h"
#include "../issm.h"

void diagnostic_core_nonlinear(Vec* pug,Mat* pKff0,Mat* pKfs0, ParameterInputs* inputs,FemModel fem){


	/*solution : */
	Vec ug=NULL; 
	Vec old_ug=NULL; 
	Vec old_uf=NULL; 
	Vec uf=NULL; 
	DataSet* loads=NULL;

	/*intermediary: */
	Mat Kgg=NULL;
	Mat Kff=NULL;
	Mat Kfs=NULL;
	Vec pg=NULL;
	Vec pf=NULL;
	int converged;
	int constraints_converged;
	int num_unstable_constraints;
	int count;

	Vec dug;
	double ndu,nu;

	/*parameters:*/
	int kflag,pflag,connectivity,numberofdofspernode;
	char* analysis_type_string=NULL;
	int analysis_type;
	char* solver_string=NULL;
	int debug=0;
	double eps_rel;

	/*Recover parameters: */
	kflag=1; pflag=1;
	fem.parameters->FindParam((void*)&connectivity,"connectivity");
	fem.parameters->FindParam((void*)&numberofdofspernode,"numberofdofspernode");
	fem.parameters->FindParam((void*)&solver_string,"solverstring");
	fem.parameters->FindParam((void*)&eps_rel,"eps_rel");
	fem.parameters->FindParam((void*)&debug,"debug");

	fem.parameters->FindParam((void*)&analysis_type_string,"analysis_type");
    analysis_type=AnalysisTypeAsEnum(analysis_type_string);

	/*Copy loads for backup: */
	loads=fem.loads->Copy();

	count=1;
	converged=0;
	for(;;){

		if (debug) _printf_("   Updating inputs\n");

		//save pointer to old velocity
		VecFree(&old_ug);old_ug=ug;
		VecFree(&old_uf);old_uf=uf;

		/*Set input parameters: */
		if(ug)ParameterInputsAddFromVec(inputs,ug,"velocity");

		/*Update parameters: */
		UpdateFromInputsx(fem.elements,fem.nodes,loads, fem.materials,inputs);

		if (debug) _printf_("   Generating matrices\n");
		//*Generate system matrices
		SystemMatricesx(&Kgg, &pg,fem.elements,fem.nodes,loads,fem.materials,kflag,pflag,connectivity,numberofdofspernode,inputs,analysis_type); 

		if (debug) _printf_("   Generating penalty matrices\n");
		//*Generate penalty system matrices
		PenaltySystemMatricesx(Kgg, pg,fem.elements,fem.nodes,loads,fem.materials,kflag,pflag,inputs,analysis_type); 

		if (debug) _printf_("   reducing matrix from g to f set\n");
		/*!Reduce matrix from g to f size:*/
		Reducematrixfromgtofx(&Kff,&Kfs,Kgg,fem.Gmn,fem.nodesets);

		/*Free ressources: */
		MatFree(&Kgg);
	
		if (debug) _printf_("   reducing load from g to f set\n");
		/*!Reduce load from g to f size: */
		Reduceloadfromgtofx(&pf, pg, fem.Gmn, Kfs, fem.ys, fem.nodesets);

		//no need for pg and Kfs anymore 
		VecFree(&pg); 
		MatFree(&Kfs);

		/*Solve: */
		if (debug) _printf_("   solving\n");
		Solverx(&uf, Kff, pf, old_uf, solver_string);
	
		//no need for Kff and pf anymore
		MatFree(&Kff);VecFree(&pf);

		
		if (debug) _printf_("   merging solution from f to g set\n");
		//Merge back to g set
		Mergesolutionfromftogx(&ug, uf,fem.Gmn,fem.ys,fem.nodesets);

		//Deal with penalty loads
		if (debug) _printf_("   penalty constraints\n");
		ParameterInputsAddFromVec(inputs,ug,"velocity");
		
		PenaltyConstraintsx(&constraints_converged, &num_unstable_constraints, fem.elements,fem.nodes,loads,fem.materials,inputs,analysis_type); 

		//Figure out if convergence is reached.
		if(count>=2){
			VecDuplicate(old_ug,&dug);VecCopy(old_ug,dug); VecAYPX(dug,-1.0,ug);
			VecNorm(dug,NORM_2,&ndu); VecNorm(ug,NORM_2,&nu);VecFree(&dug);


			if((ndu/nu)<eps_rel){
				if (constraints_converged) converged=1;
			}
		
			if (debug){
				_printf_("%s%g%s%g\n","   Convergence criterion: norm(du)/norm(u)=",ndu/nu," > ",eps_rel);
			}
		}

		/*Increase count: */
		count++;
		if(converged==1)break;

	}

	//more output might be needed, when running in cielocontrol.c
	if(pKff0){

		/*Set input parameters: */
		ParameterInputsAddFromVec(inputs,ug,"velocity");
	
		kflag=1; pflag=0; //stiffness generation only
	
		SystemMatricesx(&Kgg, &pg,fem.elements,fem.nodes,fem.loads,fem.materials,kflag,pflag,connectivity,numberofdofspernode,inputs,analysis_type); 
	
		Reducematrixfromgtofx(&Kff,&Kfs,Kgg,fem.Gmn,fem.nodesets);
		
		MatFree(&Kgg);VecFree(&pg);

	}

	/*Delete loads: */
	delete loads;
	
	/*Assign output pointers: */
	*pug=ug;
	if(pKff0)*pKff0=Kff;
	if(pKfs0)*pKfs0=Kfs;

}

