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

int ExtrapolationAnalysis::DofsPerNode(int** doflist,int domaintype,int approximation){/*{{{*/
	return 1;
}
/*}}}*/
void ExtrapolationAnalysis::UpdateParameters(Parameters* parameters,IoModel* iomodel,int solution_enum,int analysis_enum){/*{{{*/
	//do nothing for now
	return;
}
/*}}}*/
void ExtrapolationAnalysis::UpdateElements(Elements* elements,IoModel* iomodel,int analysis_counter,int analysis_type){/*{{{*/
	int    finiteelement;

	/*Finite element type*/
	finiteelement = P1Enum;

	/*Update elements: */
	int counter=0;
	for(int i=0;i<iomodel->numberofelements;i++){
		if(iomodel->my_elements[i]){
			Element* element=(Element*)elements->GetObjectByOffset(counter);
			element->Update(i,iomodel,analysis_counter,analysis_type,finiteelement);
			counter++;
		}
	}
	if(iomodel->domaintype!=Domain2DhorizontalEnum){
		iomodel->FetchDataToInput(elements,MeshVertexonbaseEnum);
		iomodel->FetchDataToInput(elements,MeshVertexonsurfaceEnum);
	}
}
/*}}}*/
void ExtrapolationAnalysis::CreateNodes(Nodes* nodes,IoModel* iomodel){/*{{{*/
	int finiteelement=P1Enum;
	if(iomodel->domaintype!=Domain2DhorizontalEnum) iomodel->FetchData(2,MeshVertexonbaseEnum,MeshVertexonsurfaceEnum);
	::CreateNodes(nodes,iomodel,ExtrapolationAnalysisEnum,finiteelement);
	iomodel->DeleteData(2,MeshVertexonbaseEnum,MeshVertexonsurfaceEnum);
}
/*}}}*/
void ExtrapolationAnalysis::CreateConstraints(Constraints* constraints,IoModel* iomodel){/*{{{*/
	// do nothing for now
	return;
}
/*}}}*/
void ExtrapolationAnalysis::CreateLoads(Loads* loads, IoModel* iomodel){/*{{{*/
	//	do nothing for now
	return;
}/*}}}*/

/*Finite element Analysis*/
void ExtrapolationAnalysis::Core(FemModel* femmodel){/*{{{*/

	/* Intermediaries */
	bool save_results;
	int extvar_enum; 
   femmodel->parameters->FindParam(&extvar_enum, ExtrapolationVariableEnum);

	/*activate formulation: */
	femmodel->SetCurrentConfiguration(ExtrapolationAnalysisEnum);

	if(VerboseSolution()) _printf0_("extrapolation of " << EnumToStringx(extvar_enum) << ": call computational core:\n");
	solutionsequence_linear(femmodel);

	save_results=true;
	if(save_results){
		if(VerboseSolution()) _printf0_("   saving results\n");
		int outputs[2] = {VxEnum,VyEnum};
		femmodel->RequestedOutputsx(&femmodel->results,&outputs[0],2);
	}
}/*}}}*/
ElementVector* ExtrapolationAnalysis::CreateDVector(Element* element){/*{{{*/
	/*Default, return NULL*/
	return NULL;
}/*}}}*/
ElementMatrix* ExtrapolationAnalysis::CreateJacobianMatrix(Element* element){/*{{{*/
	/* Jacobian required for the Newton solver */
	_error_("not implemented yet");
}/*}}}*/
ElementMatrix* ExtrapolationAnalysis::CreateKMatrix(Element* element){/*{{{*/

	if(!element->IsOnBase()) return NULL;
	Element* basalelement = element->SpawnBasalElement();

	/*Intermediaries */
	int		   domaintype,dim;
	int        i,row,col,stabilization;
	bool	   extrapolatebydiffusion = true;
	IssmDouble Jdet,D_scalar,h;
	IssmDouble norm_dlsf;
	IssmDouble hx,hy,hz,kappa;
	IssmDouble* xyz_list = NULL;

	/*Get problem dimension*/
	basalelement->FindParam(&domaintype,DomainTypeEnum);
	switch(domaintype){
		case Domain2DverticalEnum:   dim = 1; break;
		case Domain2DhorizontalEnum: dim = 2; break;
		case Domain3DEnum:           dim = 2; break;
		default: _error_("mesh "<<EnumToStringx(domaintype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes = basalelement->GetNumberOfNodes();

	/*Initialize Element vector and other vectors*/
	ElementMatrix* Ke     = basalelement->NewElementMatrix();
	IssmDouble*    B      = xNew<IssmDouble>(dim*numnodes);
	IssmDouble*    Bprime = xNew<IssmDouble>(dim*numnodes);
	IssmDouble*		D	  = xNew<IssmDouble>(dim*dim);
	IssmDouble*		dlsf  = xNew<IssmDouble>(dim);
	IssmDouble*		normal= xNew<IssmDouble>(dim);

	/*Retrieve all inputs and parameters*/
	Input* lsf_slopex_input=basalelement->GetInput(LevelsetfunctionSlopeXEnum); _assert_(lsf_slopex_input);
	Input* lsf_slopey_input=basalelement->GetInput(LevelsetfunctionSlopeYEnum); _assert_(lsf_slopey_input);
	basalelement->GetVerticesCoordinates(&xyz_list);
	h = basalelement->CharacteristicLength();

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss=basalelement->NewGauss(2);
	for(int ig=gauss->begin();ig<gauss->end();ig++){/*{{{*/
		gauss->GaussPoint(ig);

		basalelement->JacobianDeterminant(&Jdet,xyz_list,gauss);
		GetB(B,basalelement,xyz_list,gauss);
		GetBprime(Bprime,basalelement,xyz_list,gauss);
		
		D_scalar=gauss->weight*Jdet;

		if(extrapolatebydiffusion){
			/* diffuse values outward */
			for(row=0;row<dim;row++)
				for(col=0;col<dim;col++)
					if(row==col)
						D[row*dim+col] = D_scalar;
					else
						D[row*dim+col] = 0.;

			TripleMultiply(Bprime,dim,numnodes,1,
					D,dim,dim,0,
					Bprime,dim,numnodes,0,
					&Ke->values[0],1);
		}
		else{
			/* extrapolate values along normal */
			/* Get normal on ice boundary */
			lsf_slopex_input->GetInputValue(&dlsf[0],gauss);
			lsf_slopey_input->GetInputValue(&dlsf[1],gauss);
			norm_dlsf=0.;
			for(i=0;i<dim;i++)	norm_dlsf+=dlsf[i]*dlsf[i]; 
			norm_dlsf=sqrt(norm_dlsf); _assert_(norm_dlsf>0.);

			if(norm_dlsf>0.)
				for(i=0;i<dim;i++)	normal[i]=dlsf[i]/norm_dlsf;
			else
				for(i=0;i<dim;i++)	normal[i]=0.;
			
			for(row=0;row<dim;row++)
				for(col=0;col<dim;col++)
					if(row==col)
						D[row*dim+col]=D_scalar*normal[row];
					else
						D[row*dim+col]=0.;
			TripleMultiply(B,dim,numnodes,1,
						D,dim,dim,0,
						Bprime,dim,numnodes,0,
						&Ke->values[0],1);

			/* Stabilization *//*{{{*/
			stabilization=1;
			if (stabilization==0){/* no stabilization, do nothing*/}
			else if(stabilization==1){
				/* Artificial Diffusion */
				basalelement->ElementSizes(&hx,&hy,&hz);
				h=sqrt( pow(hx*normal[0],2) + pow(hy*normal[1],2));
				kappa=h/2.+1.e-14; 
				for(row=0;row<dim;row++)
					for(col=0;col<dim;col++)
						if(row==col)
							D[row*dim+col]=D_scalar*kappa;
						else
							D[row*dim+col]=0.;
				TripleMultiply(Bprime,dim,numnodes,1,
							D,dim,dim,0,
							Bprime,dim,numnodes,0,
							&Ke->values[0],1);
			}
			else if(stabilization==2){
				/*Streamline upwinding - do not use this for extrapolation: yields oscillating results due to smoothing along normal, not across */
				for(row=0;row<dim;row++)
					for(col=0;col<dim;col++)
						D[row*dim+col]=h/(2.*1.)*normal[row]*normal[col];

				TripleMultiply(Bprime,dim,numnodes,1,
							D,dim,dim,0,
							Bprime,dim,numnodes,0,
							&Ke->values[0],1);
			}/*}}}*/
		}
	}/*}}}*/

	/*Clean up and return*/
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(B);
	xDelete<IssmDouble>(Bprime);
	xDelete<IssmDouble>(D);
	xDelete<IssmDouble>(dlsf);
	xDelete<IssmDouble>(normal);
	delete gauss;
	if(domaintype!=Domain2DhorizontalEnum){basalelement->DeleteMaterials(); delete basalelement;};
	return Ke;

}/*}}}*/
ElementVector* ExtrapolationAnalysis::CreatePVector(Element* element){/*{{{*/

	if(!element->IsOnBase()) return NULL;
	Element* basalelement = element->SpawnBasalElement();

	/*Intermediaries */
	int i, domaintype;
	
	/*Fetch number of nodes */
	int numnodes = basalelement->GetNumberOfNodes();

	/*Initialize Element vector*/
	ElementVector* pe = basalelement->NewElementVector();
	for(i=0;i<numnodes;i++) 
		pe->values[i]=0.; 

	basalelement->FindParam(&domaintype,DomainTypeEnum);
	if(domaintype!=Domain2DhorizontalEnum){basalelement->DeleteMaterials(); delete basalelement;};
	return pe;
}/*}}}*/
void ExtrapolationAnalysis::GetSolutionFromInputs(Vector<IssmDouble>* solution,Element* element){/*{{{*/
	_error_("not implemented yet");
}/*}}}*/
void ExtrapolationAnalysis::GradientJ(Vector<IssmDouble>* gradient,Element* element,int control_type,int control_index){/*{{{*/
	_error_("Not implemented yet");
}/*}}}*/
void ExtrapolationAnalysis::InputUpdateFromSolution(IssmDouble* solution,Element* element){/*{{{*/

	int domaintype, extrapolationvariable;
	element->FindParam(&domaintype,DomainTypeEnum);
	element->FindParam(&extrapolationvariable, ExtrapolationVariableEnum);
	switch(domaintype){
		case Domain2DhorizontalEnum:
			element->InputUpdateFromSolutionOneDof(solution,extrapolationvariable);
			break;
		case Domain3DEnum:
			element->InputUpdateFromSolutionOneDofCollapsed(solution,extrapolationvariable);
			break;
		default: _error_("mesh "<<EnumToStringx(domaintype)<<" not supported yet");
	}
}/*}}}*/
void ExtrapolationAnalysis::GetB(IssmDouble* B,Element* element,IssmDouble* xyz_list,Gauss* gauss){/*{{{*/
	/*Compute B  matrix. B=[B1 B2 B3] where Bi is of size 3*NDOF2. 
	 * For node i, Bi can be expressed in the actual coordinate system
	 * by: 
	 *       Bi=[ N ]
	 *          [ N ]
	 * where N is the finiteelement function for node i.
	 *
	 * We assume B_prog has been allocated already, of size: 2x(NDOF1*numnodes)
	 */

	/*Fetch number of nodes for this finite element*/
	int numnodes = element->GetNumberOfNodes();

	/*Get nodal functions*/
	IssmDouble* basis=xNew<IssmDouble>(numnodes);
	element->NodalFunctions(basis,gauss);

	/*Build B: */
	for(int i=0;i<numnodes;i++){
		B[numnodes*0+i] = basis[i];
		B[numnodes*1+i] = basis[i];
	}

	/*Clean-up*/
	xDelete<IssmDouble>(basis);
}/*}}}*/
void ExtrapolationAnalysis::GetBprime(IssmDouble* Bprime,Element* element,IssmDouble* xyz_list,Gauss* gauss){/*{{{*/
	/*Compute B'  matrix. B'=[B1' B2' B3'] where Bi' is of size 3*NDOF2. 
	 * For node i, Bi' can be expressed in the actual coordinate system
	 * by: 
	 *       Bi_prime=[ dN/dx ]
	 *                [ dN/dy ]
	 * where N is the finiteelement function for node i.
	 *
	 * We assume B' has been allocated already, of size: 3x(NDOF2*numnodes)
	 */

	/*Fetch number of nodes for this finite element*/
	int numnodes = element->GetNumberOfNodes();

	/*Get nodal functions derivatives*/
	IssmDouble* dbasis=xNew<IssmDouble>(2*numnodes);
	element->NodalFunctionsDerivatives(dbasis,xyz_list,gauss);

	/*Build B': */
	for(int i=0;i<numnodes;i++){
		Bprime[numnodes*0+i] = dbasis[0*numnodes+i];
		Bprime[numnodes*1+i] = dbasis[1*numnodes+i];
	}

	/*Clean-up*/
	xDelete<IssmDouble>(dbasis);

}/*}}}*/
void ExtrapolationAnalysis::SetConstraintsOnIce(Element* element){/*{{{*/

	int numnodes=element->GetNumberOfNodes();	

	/* Intermediaries */
	int extvar_enum;
	IssmDouble phi,value;
	Node* node = NULL;

	/* Get parameters */
	element->FindParam(&extvar_enum, ExtrapolationVariableEnum);
	
	Input* levelset_input=element->GetInput(MaskIceLevelsetEnum); _assert_(levelset_input);
	Input* extvar_input=element->GetInput(extvar_enum); _assert_(extvar_input);

	Gauss* gauss=element->NewGauss();
	for(int in=0;in<numnodes;in++){
		gauss->GaussNode(element->GetElementType(),in);
		node=element->GetNode(in);
		levelset_input->GetInputValue(&phi,gauss);
		if(node->IsActive()){
			if(phi<=0.){
				/* if ice, set dirichlet BC */
				extvar_input->GetInputValue(&value,gauss);
				node->ApplyConstraint(0,value);
			}
			else {
				/* no ice, set no spc */
				node->DofInFSet(0); 
			}
		}
	}
	delete gauss;
}/*}}}*/
void ExtrapolationAnalysis::UpdateConstraints(FemModel* femmodel){/*{{{*/

	for(int i=0;i<femmodel->elements->Size();i++){
		Element* element=dynamic_cast<Element*>(femmodel->elements->GetObjectByOffset(i));
		this->SetConstraintsOnIce(element);
	}
}/*}}}*/
