/*!\file: solver_nonlinear.cpp
 * \brief: core of a non-linear solution, using fixed-point method 
 */ 

#include "../toolkits/toolkits.h"
#include "../objects/objects.h"
#include "../io/io.h"
#include "../EnumDefinitions/EnumDefinitions.h"
#include "../modules/modules.h"
#include "../solutions/solutions.h"
#include "./solvers.h"

void solver_nonlinear(FemModel* femmodel,bool conserve_loads){

	/*intermediary: */
	Mat Kff = NULL, Kfs   = NULL;
	Vec ug  = NULL, old_ug = NULL;
	Vec uf  = NULL, old_uf = NULL;
	Vec pf  = NULL;
	Vec df  = NULL;
	Vec ys  = NULL;
	
	Loads* loads=NULL;
	bool converged;
	int constraints_converged;
	int num_unstable_constraints;
	int count;

	/*parameters:*/
	int min_mechanical_constraints;
	int max_nonlinear_iterations;
	int  configuration_type;

	/*Recover parameters: */
	femmodel->parameters->FindParam(&min_mechanical_constraints,DiagnosticRiftPenaltyThresholdEnum);
	femmodel->parameters->FindParam(&max_nonlinear_iterations,DiagnosticMaxiterEnum);
	femmodel->parameters->FindParam(&configuration_type,ConfigurationTypeEnum);
	UpdateConstraintsx(femmodel->nodes,femmodel->constraints,femmodel->parameters);

	/*Were loads requested as output? : */
	if(conserve_loads) loads=(Loads*)femmodel->loads->Copy(); //protect loads from being modified by the solution
	else               loads=(Loads*)femmodel->loads;         //modify loads  in this solution

	count=1;
	converged=false;

	/*Start non-linear iteration using input velocity: */
	GetSolutionFromInputsx(&ug, femmodel->elements, femmodel->nodes, femmodel->vertices, loads, femmodel->materials, femmodel->parameters);
	Reducevectorgtofx(&uf, ug, femmodel->nodes,femmodel->parameters);

	//Update once again the solution to make sure that vx and vxold are similar (for next step in transient or steadystate)
	InputUpdateFromConstantx( femmodel->elements,femmodel->nodes, femmodel->vertices, femmodel->loads, femmodel->materials, femmodel->parameters,converged,ConvergedEnum);
	InputUpdateFromSolutionx(femmodel->elements,femmodel->nodes, femmodel->vertices, femmodel->loads, femmodel->materials, femmodel->parameters,ug);

	for(;;){

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

		SystemMatricesx(&Kff, &Kfs, &pf, &df, NULL,femmodel->elements,femmodel->nodes,femmodel->vertices,loads,femmodel->materials,femmodel->parameters);
		CreateNodalConstraintsx(&ys,femmodel->nodes,configuration_type);
		Reduceloadx(pf, Kfs, ys); MatFree(&Kfs);
		Solverx(&uf, Kff, pf, old_uf, df, femmodel->parameters);
		Mergesolutionfromftogx(&ug, uf,ys,femmodel->nodes,femmodel->parameters);VecFree(&ys);

		convergence(&converged,Kff,pf,uf,old_uf,femmodel->parameters); MatFree(&Kff);VecFree(&pf); VecFree(&df);
		InputUpdateFromConstantx( femmodel->elements,femmodel->nodes, femmodel->vertices, femmodel->loads, femmodel->materials, femmodel->parameters,converged,ConvergedEnum);
		InputUpdateFromSolutionx( femmodel->elements,femmodel->nodes, femmodel->vertices, femmodel->loads, femmodel->materials, femmodel->parameters,ug);

		ConstraintsStatex(&constraints_converged, &num_unstable_constraints, femmodel->elements,femmodel->nodes,femmodel->vertices,loads,femmodel->materials,femmodel->parameters);
		_printf_(VerboseConvergence(),"   number of unstable constraints: %i\n",num_unstable_constraints);

		//rift convergence
		if (!constraints_converged) {
			if (converged){
				if (num_unstable_constraints <= min_mechanical_constraints) converged=true;
				else converged=false;
			}
		}

		/*Increase count: */
		count++;
		if(converged==true)break;
		if(count>=max_nonlinear_iterations){
			_printf_(true,"   maximum number of iterations (%i) exceeded\n",max_nonlinear_iterations); 
			converged=true;
		InputUpdateFromConstantx( femmodel->elements,femmodel->nodes, femmodel->vertices, femmodel->loads, femmodel->materials, femmodel->parameters,converged,ConvergedEnum);
		InputUpdateFromSolutionx( femmodel->elements,femmodel->nodes, femmodel->vertices, femmodel->loads, femmodel->materials, femmodel->parameters,ug);
			break;
		}
	}

	_printf_(VerboseConvergence(),"\n   total number of iterations: %i\n",count-1);

	/*clean-up*/
	if(conserve_loads) delete loads;
	VecFree(&uf);
	VecFree(&ug);
	VecFree(&old_ug);
	VecFree(&old_uf);
}
