/*!\file: control_core.cpp
 * \brief: core of the control solution 
 */ 

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

/*Local prototypes*/
bool controlconvergence(IssmDouble J, IssmDouble tol_cm);

void control_core(FemModel* femmodel){

	int     i;

	/*parameters: */
	int        num_controls,num_responses;
	int        nsteps;
	IssmDouble tol_cm;
	bool       cm_gradient;
	int        dim;
	int        solution_type;
	bool       isFS;
	bool       dakota_analysis = false;

	int        *control_type   = NULL;
	IssmDouble *responses      = NULL;
	int        *step_responses = NULL;
	IssmDouble *maxiter        = NULL;
	IssmDouble *cm_jump        = NULL;

	/*intermediary: */
	IssmDouble search_scalar = 1.;
	OptArgs    optargs;
	OptPars    optpars;

	/*Solution and Adjoint core pointer*/
	void (*solutioncore)(FemModel*) = NULL;
	void (*adjointcore)(FemModel*)  = NULL;

	/*output: */
	IssmDouble* J=NULL;

	/*Recover parameters used throughout the solution*/
	femmodel->parameters->FindParam(&num_controls,InversionNumControlParametersEnum);
	femmodel->parameters->FindParam(&num_responses,InversionNumCostFunctionsEnum);
	femmodel->parameters->FindParam(&control_type,NULL,InversionControlParametersEnum);
	femmodel->parameters->FindParam(&responses,NULL,NULL,InversionCostFunctionsEnum);
	femmodel->parameters->FindParam(&nsteps,InversionNstepsEnum);
	femmodel->parameters->FindParam(&maxiter,NULL,InversionMaxiterPerStepEnum);
	femmodel->parameters->FindParam(&cm_jump,NULL,InversionStepThresholdEnum);
	femmodel->parameters->FindParam(&tol_cm,InversionCostFunctionThresholdEnum);
	femmodel->parameters->FindParam(&cm_gradient,InversionGradientOnlyEnum);
	femmodel->parameters->FindParam(&dim,MeshDimensionEnum);
	femmodel->parameters->FindParam(&solution_type,SolutionTypeEnum);
	femmodel->parameters->FindParam(&isFS,FlowequationIsFSEnum);
	femmodel->parameters->FindParam(&dakota_analysis,QmuIsdakotaEnum);
	femmodel->parameters->SetParam(false,SaveResultsEnum);

	/*out of solution_type, figure out solution core and adjoint function pointer*/
	CorePointerFromSolutionEnum(&solutioncore,femmodel->parameters,solution_type);
	AdjointCorePointerFromSolutionEnum(&adjointcore,solution_type);

	/*Launch once a complete solution to set up all inputs*/
	if(VerboseControl()) _printf0_("   preparing initial solution\n");
	if(isFS) solutioncore(femmodel);

	/*Initialize responses: */
	J=xNew<IssmDouble>(nsteps);
	step_responses=xNew<int>(num_responses);

	/*Initialize some of the BrentSearch arguments: */
	optargs.femmodel=femmodel;
	optpars.xmin=0; optpars.xmax=1;

	/*Start looping: */
	for(int n=0;n<nsteps;n++){

		/*Display info*/
		if(VerboseControl()) _printf0_("\n" << "   control method step " << n+1 << "/" << nsteps << "\n");
		for(i=0;i<num_responses;i++) step_responses[i]=reCast<int,IssmDouble>(responses[n*num_responses+i]);
		femmodel->parameters->SetParam(step_responses,1,num_responses,StepResponsesEnum);

		/*In steady state inversion, compute new temperature field now*/
		if(solution_type==SteadystateSolutionEnum) solutioncore(femmodel);

		if(VerboseControl()) _printf0_("   compute adjoint state:\n");
		adjointcore(femmodel);
		gradient_core(femmodel,n,search_scalar==0.);

		/*Return gradient if asked: */
		if (cm_gradient){
			InputToResultx(femmodel,GradientEnum);
			goto cleanup_and_return;
		}

		if(VerboseControl()) _printf0_("   optimizing along gradient direction\n");
		optpars.maxiter=reCast<int,IssmDouble>(maxiter[n]); optpars.cm_jump=cm_jump[n];
		BrentSearch(&search_scalar,J+n,&optpars,&objectivefunction,&optargs);

		if(VerboseControl()) _printf0_("   updating parameter using optimized search scalar\n"); //true means update save controls
		InputControlUpdatex(femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters,search_scalar,true);

		if(controlconvergence(J[n],tol_cm)) break;
	}

	if(VerboseControl()) _printf0_("   preparing final solution\n");
	femmodel->parameters->SetParam(true,SaveResultsEnum);
	solutioncore(femmodel);

	/*some results not computed by steadystate_core or stressbalance_core: */
	if(!dakota_analysis){ //do not save this if we are running the control core from a qmu run!
		for(i=0;i<num_controls;i++) InputToResultx(femmodel,control_type[i]);

		#ifdef _HAVE_ADOLC_
		IssmPDouble* J_passive=xNew<IssmPDouble>(nsteps);
		for(i=0;i<nsteps;i++) J_passive[i]=reCast<IssmPDouble>(J[i]);
		femmodel->results->AddObject(new GenericExternalResult<IssmPDouble*>(femmodel->results->Size()+1,JEnum,J_passive,nsteps,1,1,0));
		xDelete<IssmPDouble>(J_passive);
		#else
		femmodel->results->AddObject(new GenericExternalResult<IssmPDouble*>(femmodel->results->Size()+1,JEnum,J,nsteps,1,1,0));
		#endif
	}

	cleanup_and_return:
	/*Free ressources: */
	xDelete<int>(control_type);
	xDelete<int>(step_responses);
	xDelete<IssmDouble>(maxiter);
	xDelete<IssmDouble>(responses);
	xDelete<IssmDouble>(cm_jump);
	xDelete<IssmDouble>(J);
}
bool controlconvergence(IssmDouble J, IssmDouble tol_cm){

	bool converged=false;

	/*Has convergence been reached?*/
	if (!xIsNan<IssmDouble>(tol_cm) && J<tol_cm){
		converged=true;
		if(VerboseConvergence()) _printf0_("      Convergence criterion reached: J = " << J << " < " << tol_cm);
	}

	return converged;
}
