/*!\file: solutionsequence_hydro_nonlinear.cpp
 * \brief: core of the hydro solution 
 */ 

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

void solutionsequence_hydro_nonlinear(FemModel* femmodel){
	/*solution : */
	Vector<IssmDouble>* ug_sed=NULL; 
	Vector<IssmDouble>* uf_sed=NULL; 
	Vector<IssmDouble>* uf_sed_sub_iter=NULL; 
	Vector<IssmDouble>* ug_sed_main_iter=NULL;

	Vector<IssmDouble>* ug_epl=NULL; 
	Vector<IssmDouble>* uf_epl=NULL;
	Vector<IssmDouble>* uf_epl_sub_iter=NULL; 
	Vector<IssmDouble>* ug_epl_main_iter=NULL;

	Vector<IssmDouble>* ys=NULL; 
	Vector<IssmDouble>* dug=NULL;

	Matrix<IssmDouble>* Kff=NULL;
	Matrix<IssmDouble>* Kfs=NULL;
	Vector<IssmDouble>* pf=NULL;
	Vector<IssmDouble>* df=NULL;

	bool       sedconverged,eplconverged,hydroconverged;
	bool       isefficientlayer;
	int        constraints_converged;
	int        num_unstable_constraints;
	int        sedcount,eplcount,hydrocount;
	int        hydro_maxiter;
	IssmDouble sediment_kmax,time;
	IssmDouble eps_hyd;
	IssmDouble ndu_sed,nu_sed;
	IssmDouble ndu_epl,nu_epl;

	/*Recover parameters: */
	femmodel->SetCurrentConfiguration(HydrologyDCInefficientAnalysisEnum);
	femmodel->parameters->FindParam(&isefficientlayer,HydrologydcIsefficientlayerEnum);
	femmodel->parameters->FindParam(&eps_hyd,HydrologydcRelTolEnum);
	femmodel->parameters->FindParam(&time,TimeEnum);
	hydro_maxiter=150;
	hydrocount=1;
	hydroconverged=false;

	/*Retrieve inputs as the initial state for the non linear iteration*/
	//femmodel->SetCurrentConfiguration(HydrologyDCInefficientAnalysisEnum);
	GetSolutionFromInputsx(&ug_sed,femmodel);

	if(isefficientlayer) {
		femmodel->SetCurrentConfiguration(HydrologyDCEfficientAnalysisEnum);
		GetSolutionFromInputsx(&ug_epl,femmodel);
	}

	/*Iteration on the two layers*/
	for(;;){
		sedcount=1;
		eplcount=1;
		//save pointer to old velocity
		ug_sed_main_iter=ug_sed->Duplicate();
		ug_sed->Copy(ug_sed_main_iter);
		if(isefficientlayer){
			ug_epl_main_iter=ug_epl->Duplicate();
			ug_epl->Copy(ug_epl_main_iter);
		}

		femmodel->SetCurrentConfiguration(HydrologyDCInefficientAnalysisEnum);
		InputUpdateFromConstantx(femmodel,true,ResetPenaltiesEnum);
		InputUpdateFromConstantx(femmodel,false,ConvergedEnum);
		femmodel->UpdateConstraintsx();
		femmodel->parameters->SetParam(HydrologySedimentEnum,HydrologyLayerEnum);

		/*Iteration on the sediment layer*/
		sedconverged=false;
		for(;;){
			femmodel->HydrologyTransferx();
			SystemMatricesx(&Kff,&Kfs,&pf,&df,&sediment_kmax,femmodel);
			CreateNodalConstraintsx(&ys,femmodel->nodes,HydrologyDCInefficientAnalysisEnum);
			Reduceloadx(pf,Kfs,ys); delete Kfs;
			delete uf_sed;
			Solverx(&uf_sed,Kff,pf,uf_sed_sub_iter,df,femmodel->parameters);
			delete Kff; delete pf; delete df;
			delete uf_sed_sub_iter;
			uf_sed_sub_iter=uf_sed->Duplicate();
			uf_sed->Copy(uf_sed_sub_iter);
			delete ug_sed;
			Mergesolutionfromftogx(&ug_sed,uf_sed,ys,femmodel->nodes,femmodel->parameters); delete ys;
			InputUpdateFromSolutionx(femmodel,ug_sed);
			ConstraintsStatex(&constraints_converged,&num_unstable_constraints,femmodel);

			if (!sedconverged){
				if(VerboseConvergence()) _printf0_("   # Sediment unstable constraints = " << num_unstable_constraints << "\n");
				if(num_unstable_constraints==0) sedconverged = true;
				if (sedcount>=hydro_maxiter){
					_error_("   maximum number of Sediment iterations (" << hydro_maxiter << ") exceeded");
				}
			}
			sedcount++;

			if(sedconverged){
				femmodel->parameters->SetParam(sediment_kmax,HydrologySedimentKmaxEnum);
				InputUpdateFromConstantx(femmodel,sedconverged,ConvergedEnum);
				InputUpdateFromSolutionx(femmodel,ug_sed);
				InputUpdateFromConstantx(femmodel,sediment_kmax,HydrologySedimentKmaxEnum);
				break;
			}
		}

		/*Second layer*/
		if(isefficientlayer){
			femmodel->SetCurrentConfiguration(HydrologyDCEfficientAnalysisEnum);
			InputUpdateFromConstantx(femmodel,true,ResetPenaltiesEnum);
			InputUpdateFromConstantx(femmodel,false,ConvergedEnum);
			femmodel->HydrologyEPLupdateDomainx();
			femmodel->parameters->SetParam(HydrologyEfficientEnum,HydrologyLayerEnum);

			/*Iteration on the EPL layer*/
			eplconverged = false;
			for(;;){
				femmodel->HydrologyTransferx();
				SystemMatricesx(&Kff,&Kfs,&pf,&df,NULL,femmodel);
				CreateNodalConstraintsx(&ys,femmodel->nodes,HydrologyDCEfficientAnalysisEnum);
				Reduceloadx(pf,Kfs,ys); delete Kfs;
				delete uf_epl;
				Solverx(&uf_epl,Kff,pf,uf_epl_sub_iter,df,femmodel->parameters);
				delete Kff; delete pf; delete df;
				delete uf_epl_sub_iter; 
				uf_epl_sub_iter=uf_epl->Duplicate();
				uf_epl->Copy(uf_epl_sub_iter);
				delete ug_epl; 
				Mergesolutionfromftogx(&ug_epl,uf_epl,ys,femmodel->nodes,femmodel->parameters); delete ys;
				InputUpdateFromSolutionx(femmodel,ug_epl);
				ConstraintsStatex(&constraints_converged,&num_unstable_constraints,femmodel);
				femmodel->HydrologyEPLupdateDomainx();			

				if (!eplconverged){
					if(VerboseConvergence()) _printf0_("   # EPL unstable constraints = " << num_unstable_constraints << "\n");
					if(num_unstable_constraints==0) eplconverged = true;
					if (eplcount>=hydro_maxiter){
						_error_("   maximum number of EPL iterations (" << hydro_maxiter << ") exceeded");
					}
				}
				eplcount++;

				if(eplconverged){
					InputUpdateFromConstantx(femmodel,eplconverged,ConvergedEnum);
					InputUpdateFromConstantx(femmodel,sediment_kmax,MeltingOffsetEnum);
					InputUpdateFromSolutionx(femmodel,ug_epl);
					break;
				}
			}
		}
		/*System convergence check*/
		if(!hydroconverged){
			//compute norm(du)/norm(u)
			dug=ug_sed->Duplicate(); _assert_(dug);
			ug_sed_main_iter->Copy(dug);	
			dug->AYPX(ug_sed,-1.0);
			ndu_sed=dug->Norm(NORM_TWO); 
			delete dug;
			nu_sed=ug_sed_main_iter->Norm(NORM_TWO);
			delete ug_sed_main_iter;
			if (xIsNan<IssmDouble>(ndu_sed) || xIsNan<IssmDouble>(nu_sed)) _error_("Sed convergence criterion is NaN!");
			if (!xIsNan<IssmDouble>(eps_hyd)){
				if (!isefficientlayer){
					if ((ndu_sed/nu_sed)<eps_hyd){
						if(VerboseConvergence()) _printf0_(setw(50) << left << "   Converged after, " << hydrocount << " iterations \n");
						hydroconverged=true;
					}
					else{ 
						if(VerboseConvergence()) _printf0_(setw(50) << left << "   Sediment Convergence criterion:" << ndu_sed/nu_sed*100 << " > " << eps_hyd*100 << " %\n");
						hydroconverged=false;
					}
				}
				else{
					dug=ug_epl->Duplicate();_assert_(dug); 
					ug_epl_main_iter->Copy(dug);_assert_(ug_epl_main_iter); 
					dug->AYPX(ug_epl,-1.0);
					ndu_epl=dug->Norm(NORM_TWO); 
					delete dug;
					nu_epl=ug_epl_main_iter->Norm(NORM_TWO);
					delete ug_epl_main_iter;
					if (xIsNan<IssmDouble>(ndu_epl) || xIsNan<IssmDouble>(nu_epl)) _error_("EPL convergence criterion is NaN!");
					if (ndu_epl==0.0 && nu_epl==0.0) nu_epl=1.0e-6; /*Hacking the case where the EPL is used but empty*/
					if ((ndu_epl/nu_epl)<eps_hyd && (ndu_sed/nu_sed)<(eps_hyd*10)){
						if (VerboseConvergence()) _printf0_(setw(50) << left << "   Converged after, " << hydrocount << " iterations \n");
						hydroconverged=true;
					}
					else{ 
						if(VerboseConvergence()) _printf0_(setw(50) << left << "   Sediment Convergence criterion:" << ndu_sed/nu_sed*100 << " aiming lower than " << eps_hyd*100 << " %\n");
						if(VerboseConvergence()) _printf0_(setw(50) << left << "   EPL Convergence criterion:" << ndu_epl/nu_epl*100 << " aiming lower than " << eps_hyd*100 << " %\n");
						hydroconverged=false;
					}
				}
			}
			else _printf0_(setw(50) << left << "   Convergence criterion:" << ndu_sed/nu_sed*100 << " %\n");
			if (hydrocount>=hydro_maxiter){
				_error_("   maximum number for hydrological global iterations (" << hydro_maxiter << ") exceeded");
			}
		}
		hydrocount++;
		if(hydroconverged)break;
	}

	InputUpdateFromSolutionx(femmodel,ug_sed);
	if(isefficientlayer)InputUpdateFromSolutionx(femmodel,ug_epl);

	/*Free ressources: */
	delete ug_epl;
	delete ug_sed;
	delete uf_sed;
	delete uf_epl;
	delete uf_sed_sub_iter;
	delete uf_epl_sub_iter;
	//delete dug;
}
