/*!\file: solutionsequence_la_theta.cpp
 * \brief: numerical core of la_theta solutions
 */ 

#include "../toolkits/toolkits.h"
#include "../classes/classes.h"
#include "../shared/shared.h"
#include "../modules/modules.h"

void solutionsequence_la_theta(FemModel* femmodel){

	/*intermediary: */
	Matrix<IssmDouble>*  Kff = NULL;
	Matrix<IssmDouble>*  Kfs = NULL;
	Vector<IssmDouble>*  ug  = NULL;
	Vector<IssmDouble>*  uf  = NULL;
	Vector<IssmDouble>*  pf  = NULL;
	Vector<IssmDouble>*  df  = NULL;
	Vector<IssmDouble>*  ys  = NULL;
	int  configuration_type;

	/*Create analysis*/
	StressbalanceAnalysis* analysis = new StressbalanceAnalysis();

	/*Recover parameters: */
	femmodel->parameters->FindParam(&configuration_type,ConfigurationTypeEnum);

	/*Update constraints and initialize d and tau if necessary*/
	femmodel->UpdateConstraintsx();
	analysis->InitializeXTH(femmodel->elements,femmodel->parameters);

	/*Convergence criterion*/
	int count = 0;
	femmodel->parameters->SetParam(reCast<IssmDouble>(10.),AugmentedLagrangianREnum);

	while(true){

		/*Solve KU=F*/
		SystemMatricesx(&Kff,&Kfs,&pf,&df,NULL,femmodel);
		CreateNodalConstraintsx(&ys,femmodel->nodes,configuration_type);
		Reduceloadx(pf, Kfs, ys); delete Kfs;
		Solverx(&uf, Kff, pf, NULL, df, femmodel->parameters); 
		delete Kff; delete pf; delete df;
		Mergesolutionfromftogx(&ug, uf,ys,femmodel->nodes,femmodel->parameters);delete uf; delete ys;

		/*Update solution*/
		_printf_("ug norm = " << ug->Norm(NORM_TWO) << "\n");
		InputUpdateFromSolutionx(femmodel,ug); 

		/*Update d and tau accordingly*/
		analysis->InputUpdateFromSolutionFSXTH(femmodel->elements,femmodel->parameters);

		count++;
		if(count>5) break;
	}

	/*Check for convergence*/
	//_error_("STOP");

	delete ug;  
	delete analysis;
}
