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

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

void solver_newton(FemModel* femmodel){

	/*intermediary: */
	bool   converged;
	int    num_unstable_constraints;
	int    count;
	IssmDouble kmax;
	Matrix<IssmDouble>* Kff = NULL;
	Matrix<IssmDouble>* Kfs    = NULL;
	Matrix<IssmDouble>* Jff = NULL;
	Vector<IssmDouble>* ug  = NULL;
	Vector<IssmDouble>* old_ug = NULL;
	Vector<IssmDouble>* uf  = NULL;
	Vector<IssmDouble>* old_uf = NULL;
	Vector<IssmDouble>* duf = NULL;
	Vector<IssmDouble>* pf  = NULL;
	Vector<IssmDouble>* pJf    = NULL;
	Vector<IssmDouble>* df  = NULL;
	Vector<IssmDouble>* ys  = NULL;

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

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

	count=1;
	converged=false;

	/*Start non-linear iteration using input velocity: */
	GetSolutionFromInputsx(&ug,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->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(;;){

		xdelete(&old_ug);old_ug=ug;
		xdelete(&old_uf);old_uf=uf;

		/*Solver forward model*/
		SystemMatricesx(&Kff,&Kfs,&pf,&df,NULL,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters);
		CreateNodalConstraintsx(&ys,femmodel->nodes,configuration_type);
		Reduceloadx(pf,Kfs,ys);xdelete(&Kfs);
		Solverx(&uf,Kff,pf,old_uf,df,femmodel->parameters);xdelete(&df);
		Mergesolutionfromftogx(&ug,uf,ys,femmodel->nodes,femmodel->parameters);xdelete(&ys);
		InputUpdateFromSolutionx(femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters,ug);xdelete(&ug);

		/*Check convergence*/
		convergence(&converged,Kff,pf,uf,old_uf,femmodel->parameters); 
		xdelete(&Kff); xdelete(&pf);
		if(converged==true){	
			bool max_iteration_state=false;
			int tempStep=1;
			IssmDouble tempTime=1.0;
			femmodel->results->AddObject(new BoolExternalResult(femmodel->results->Size()+1, MaxIterationConvergenceFlagEnum, max_iteration_state, tempStep, tempTime));
			break;
		}
		if(count>=max_nonlinear_iterations){
			_pprintLine_("   maximum number of Newton iterations (" << max_nonlinear_iterations << ") exceeded"); 
			bool max_iteration_state=true;
			int tempStep=1;
			IssmDouble tempTime=1.0;
			femmodel->results->AddObject(new BoolExternalResult(femmodel->results->Size()+1, MaxIterationConvergenceFlagEnum, max_iteration_state, tempStep, tempTime));
			break;
		}

		/*Prepare next iteration using Newton's method*/
		SystemMatricesx(&Kff,&Kfs,&pf,NULL,&kmax,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters);
		CreateNodalConstraintsx(&ys,femmodel->nodes,configuration_type);
		Reduceloadx(pf,Kfs,ys);   xdelete(&Kfs);

		pJf=pf->Duplicate(); Kff->MatMult(uf,pJf); xdelete(&Kff);
		pJf->Scale(-1.0); pJf->AXPY(pf,+1.0);     xdelete(&pf);

		CreateJacobianMatrixx(&Jff,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters,kmax);
		Solverx(&duf,Jff,pJf,NULL,NULL,femmodel->parameters); xdelete(&Jff); xdelete(&pJf);
		uf->AXPY(duf, 1.0); xdelete(&duf);
		Mergesolutionfromftogx(&ug,uf,ys,femmodel->nodes,femmodel->parameters);xdelete(&ys);
		InputUpdateFromSolutionx(femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters,ug);

		count++;
	}

	if(VerboseConvergence()) _pprintLine_("\n   total number of iterations: " << count-1);

	/*clean-up*/
	xdelete(&uf);
	xdelete(&ug);
	xdelete(&old_ug);
	xdelete(&old_uf);
}