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

#include <config.h>
#include "./cores.h"
#include "../toolkits/toolkits.h"
#include "../classes/classes.h"
#include "../shared/shared.h"
#include "../modules/modules.h"
#include "../solutionsequences/solutionsequences.h"

#if defined (_HAVE_M1QN3_)
/*m1qn3 prototypes*/
extern "C" void *ctonbe_; // DIS mode : Conversion
extern "C" void *ctcabe_; // DIS mode : Conversion
extern "C" void *euclid_; // Scalar product
typedef void (*SimulFunc) (long* indic,long* n, double* x, double* pf,double* g,long [],float [],void* dzs);
extern "C" void m1qn3_ (void f(long* indic,long* n, double* x, double* pf,double* g,long [],float [],void* dzs),
			void **, void **, void **,
			long *, double [], double *, double [], double*, double *,
			double *, char [], long *, long *, long *, long *, long *, long *, long [], double [], long *,
			long *, long *, long [], float [],void* );

/*Cost function prototype*/
void simul(long* indic,long* n,double* X,double* pf,double* G,long izs[1],float rzs[1],void* dzs);

void controlm1qn3_core(FemModel* femmodel){

	/*Intermediaries*/
	long         omode;
	double       f,dxmin,gttol; 
	int          maxsteps,maxiter;
	int          intn,num_controls,solution_type;
	IssmDouble  *X  = NULL;
	IssmDouble  *G  = NULL;

	/*Recover some parameters*/
	femmodel->parameters->FindParam(&solution_type,SolutionTypeEnum);
	femmodel->parameters->FindParam(&num_controls,InversionNumControlParametersEnum);
	femmodel->parameters->FindParam(&maxsteps,InversionMaxstepsEnum);
	femmodel->parameters->FindParam(&maxiter,InversionMaxiterEnum);
	femmodel->parameters->FindParam(&dxmin,InversionDxminEnum);
	femmodel->parameters->FindParam(&gttol,InversionGttolEnum);
	femmodel->parameters->SetParam(false,SaveResultsEnum);

	/*Initialize M1QN3 parameters*/
	if(VerboseControl())_printf0_("   Initialize M1QN3 parameters\n");
	SimulFunc costfuncion  = &simul;    /*Cost function address*/
	void**    prosca       = &euclid_;  /*Dot product function (euclid is the default)*/
	char      normtype[]   = "dfn";     /*Norm type: dfn = scalar product defined by prosca*/
	long      izs[5];                   /*Arrays used by m1qn3 subroutines*/
	long      iz[5];                    /*Integer m1qn3 working array of size 5*/
	float     rzs[1];                   /*Arrays used by m1qn3 subroutines*/
	long      impres       = 0;         /*verbosity level*/
	long      imode[3]     = {0};       /*scaling and starting mode, 0 by default*/
	long      indic        = 4;         /*compute f and g*/
	long      reverse      = 0;         /*reverse or direct mode*/
	long      io           = 6;         /*Channel number for the output*/

	/*Optimization criterions*/
	long      niter        = long(maxsteps); /*Maximum number of iterations*/
	long      nsim         = long(maxiter);/*Maximum number of function calls*/

	/*Get initial guess*/
	Vector<IssmDouble> *Xpetsc = NULL;
	GetVectorFromControlInputsx(&Xpetsc,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters,"value");
	X = Xpetsc->ToMPISerial();
	Xpetsc->GetSize(&intn);
	delete Xpetsc;

	/*Get problem dimension and initialize gradient and initial guess*/
	long n = long(intn);
	G = xNew<double>(n);

	/*Allocate m1qn3 working arrays (see doc)*/
	long      m   = 100;
	long      ndz = 4*n+m*(2*n+1);
	double*   dz  = xNew<double>(ndz);

	if(VerboseControl())_printf0_("   Computing initial solution\n");
	_printf0_("\n");
	_printf0_("Cost function f(x)   |  List of contributions\n");
	_printf0_("_____________________________________________\n");
	indic = 0; //no adjoint required
	simul(&indic,&n,X,&f,G,izs,rzs,(void*)femmodel);
	double f1=f;

	indic = 4; //adjoint and gradient required
	m1qn3_(costfuncion,prosca,&ctonbe_,&ctcabe_,
				&n,X,&f,G,&dxmin,&f1,
				&gttol,normtype,&impres,&io,imode,&omode,&niter,&nsim,iz,dz,&ndz,
				&reverse,&indic,izs,rzs,(void*)femmodel);

	switch(int(omode)){
		case 0:  _printf0_("   Stop requested (indic = 0)\n"); break;
		case 1:  _printf0_("   Convergence reached (gradient satisfies stopping criterion)\n"); break;
		case 2:  _printf0_("   Bad initialization\n"); break;
		case 3:  _printf0_("   Line search failure\n"); break;
		case 4:  _printf0_("   Maximum number of iterations exceeded\n");break;
		case 5:  _printf0_("   Maximum number of function calls exceeded\n"); break;
		case 6:  _printf0_("   stopped on dxmin during line search\n"); break;
		case 7:  _printf0_("   <g,d> > 0  or  <y,s> <0\n"); break;
		default: _printf0_("   Unknown end condition\n");
	}

	/*Get solution*/
	SetControlInputsFromVectorx(femmodel,X);
	ControlInputSetGradientx(femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters,G);
	femmodel->OutputControlsx(&femmodel->results);
	femmodel->results->AddObject(new GenericExternalResult<double>(JEnum,f,1,0));

	/*Finalize*/
	if(VerboseControl()) _printf0_("   preparing final solution\n");
	femmodel->parameters->SetParam(true,SaveResultsEnum);
	void (*solutioncore)(FemModel*)=NULL;
	CorePointerFromSolutionEnum(&solutioncore,femmodel->parameters,solution_type);
	solutioncore(femmodel);

	/*Clean-up and return*/
	xDelete<double>(G);
	xDelete<double>(X);
	xDelete<double>(dz);
}

/*Cost function definition*/
void simul(long* indic,long* n,double* X,double* pf,double* G,long izs[1],float rzs[1],void* dzs){

	/*Recover Femmodel*/
	int         solution_type;
	FemModel   *femmodel  = (FemModel*)dzs;

	/*Recover number of cost functions responses*/
	int num_responses;
	femmodel->parameters->FindParam(&num_responses,InversionNumCostFunctionsEnum);

	/*Constrain input vector*/
	IssmDouble  *XL = NULL;
	IssmDouble  *XU = NULL;
	GetVectorFromControlInputsx(&XL,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters,"lowerbound");
	GetVectorFromControlInputsx(&XU,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters,"upperbound");
	for(long i=0;i<*n;i++){
		if(X[i]>XU[i]) X[i]=XU[i];
		if(X[i]<XL[i]) X[i]=XL[i];
	}

	/*Update control input*/
	SetControlInputsFromVectorx(femmodel,X);

	/*Recover some parameters*/
	femmodel->parameters->FindParam(&solution_type,SolutionTypeEnum);

	/*Compute solution and adjoint*/
	void (*solutioncore)(FemModel*)=NULL;
	void (*adjointcore)(FemModel*)=NULL;
	CorePointerFromSolutionEnum(&solutioncore,femmodel->parameters,solution_type);
	solutioncore(femmodel);

	/*Compute objective function*/
	IssmDouble* Jlist = NULL;
	femmodel->CostFunctionx(pf,&Jlist,NULL);
	_printf0_("f(x) = "<<setw(12)<<setprecision(7)<<*pf<<"  |  ");

	/*Retrieve objective functions independently*/
	for(int i=0;i<num_responses;i++) _printf0_(" "<<setw(12)<<setprecision(7)<<Jlist[i]);
	_printf0_("\n");
	xDelete<IssmDouble>(Jlist);

	if(indic==0){
		/*dry run, no gradient required*/
		xDelete<IssmDouble>(XU);
		xDelete<IssmDouble>(XL);
		return;
	}

	/*Compute Adjoint*/
	AdjointCorePointerFromSolutionEnum(&adjointcore,solution_type);
	adjointcore(femmodel);

	/*Compute gradient*/
	IssmDouble* G2 = NULL;
	Gradjx(&G2,NULL,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters);
	for(long i=0;i<*n;i++) G[i] = -G2[i];
	xDelete<IssmDouble>(G2);

	/*Constrain Gradient*/
	for(long i=0;i<*n;i++){
		if(X[i]>=XU[i]) G[i]=0.;
		if(X[i]<=XL[i]) G[i]=0.;
	}

	/*Clean-up and return*/
	xDelete<IssmDouble>(XU);
	xDelete<IssmDouble>(XL);
}

#else
void controlm1qn3_core(FemModel* femmodel){
	_error_("M1QN3 not installed");
}
#endif //_HAVE_M1QN3_
