/*!\file: solver_stokescoupling_nonlinear.cpp
 * \brief: core of the coupling between stokes and macayealpattyn
 */ 

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

void solver_stokescoupling_nonlinear(FemModel* femmodel,bool conserve_loads){

	/*intermediary: */
	Matrix*  Kff_horiz = NULL;
	Matrix* Kfs_horiz   = NULL;
	Vector*  ug_horiz  = NULL;
	Vector*  uf_horiz  = NULL;
	Vector*  old_uf_horiz = NULL;
	Vector*  pf_horiz  = NULL;
	Vector*  df_horiz  = NULL;
	Matrix*  Kff_vert  = NULL;
	Matrix*  Kfs_vert  = NULL;
	Vector*  ug_vert   = NULL;
	Vector*  uf_vert   = NULL;
	Vector*  pf_vert   = NULL;
	Vector*  df_vert   = NULL;
	Vector*  ys   = 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);
	UpdateConstraintsx(femmodel->nodes,femmodel->constraints,femmodel->parameters);
	
	count=1;
	converged=false;

	/*First get ug_horiz:*/
	femmodel->SetCurrentConfiguration(DiagnosticHorizAnalysisEnum);
	GetSolutionFromInputsx(&ug_horiz, femmodel->elements, femmodel->nodes, femmodel->vertices, femmodel->loads, femmodel->materials, femmodel->parameters);
	Reducevectorgtofx(&uf_horiz, ug_horiz, femmodel->nodes,femmodel->parameters);

	for(;;){

		/*First diagnostic horiz:*/
		femmodel->SetCurrentConfiguration(DiagnosticHorizAnalysisEnum);
		femmodel->parameters->FindParam(&configuration_type,ConfigurationTypeEnum);
	
		//Update once again the solution to make sure that vx and vxold are similar (for next step in transient or steadystate)
		InputUpdateFromSolutionx(femmodel->elements,femmodel->nodes, femmodel->vertices, femmodel->loads, femmodel->materials, femmodel->parameters,ug_horiz);
		xdelete(&ug_horiz);

		//save pointer to old velocity
		xdelete(&old_uf_horiz); old_uf_horiz=uf_horiz;

		/*solve: */
		SystemMatricesx(&Kff_horiz, &Kfs_horiz, &pf_horiz, &df_horiz, NULL,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters);
		CreateNodalConstraintsx(&ys,femmodel->nodes,configuration_type);
		Reduceloadx(pf_horiz, Kfs_horiz, ys); xdelete(&Kfs_horiz);
		Solverx(&uf_horiz, Kff_horiz, pf_horiz, old_uf_horiz, df_horiz,femmodel->parameters);
		Mergesolutionfromftogx(&ug_horiz, uf_horiz,ys,femmodel->nodes,femmodel->parameters); xdelete(&ys);
		InputUpdateFromSolutionx( femmodel->elements,femmodel->nodes, femmodel->vertices, femmodel->loads, femmodel->materials, femmodel->parameters,ug_horiz);

		convergence(&converged,Kff_horiz,pf_horiz,uf_horiz,old_uf_horiz,femmodel->parameters); xdelete(&Kff_horiz); xdelete(&pf_horiz); xdelete(&df_horiz);

		/*Second compute vertical velocity: */
		femmodel->SetCurrentConfiguration(DiagnosticVertAnalysisEnum);
		femmodel->parameters->FindParam(&configuration_type,ConfigurationTypeEnum);
		
		/*solve: */
		SystemMatricesx(&Kff_vert, &Kfs_vert, &pf_vert,  &df_vert,NULL,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters);
		CreateNodalConstraintsx(&ys,femmodel->nodes,configuration_type);
		Reduceloadx(pf_vert, Kfs_vert, ys); xdelete(&Kfs_vert);
		Solverx(&uf_vert, Kff_vert, pf_vert, NULL, df_vert,femmodel->parameters); xdelete(&Kff_vert); xdelete(&pf_vert); xdelete(&df_vert);
		Mergesolutionfromftogx(&ug_vert, uf_vert,ys,femmodel->nodes,femmodel->parameters);xdelete(&uf_vert); xdelete(&ys); 
		InputUpdateFromSolutionx( femmodel->elements,femmodel->nodes, femmodel->vertices, femmodel->loads, femmodel->materials, femmodel->parameters,ug_vert);
		xdelete(&ug_vert); xdelete(&uf_vert);

		/*Increase count: */
		count++;
		if(converged==true)break;
		if(count>=max_nonlinear_iterations){
			_pprintLine_("   maximum number of iterations (" << max_nonlinear_iterations << ") exceeded"); 
			break;
		}
	}

	/*clean-up*/
	xdelete(&old_uf_horiz);
	xdelete(&uf_horiz);
	xdelete(&ug_horiz);
	xdelete(&ys);
}
