CFEM_LinearElasticity::CFEM_LinearElasticity(unsigned short val_nDim, unsigned short val_nVar,
                                   CConfig *config) : CFEM_Elasticity(val_nDim, val_nVar, config) {

	unsigned short iVar;

	if (nDim == 2){
		nodalDisplacement = new su2double* [4];	/*--- As of now, 4 is the maximum number of nodes for 2D problems ---*/
		for (iVar = 0; iVar < 4; iVar++) nodalDisplacement[iVar] = new su2double[nDim];
	}
	else if (nDim == 3){
		nodalDisplacement = new su2double* [8];	/*--- As of now, 8 is the maximum number of nodes for 3D problems ---*/
		for (iVar = 0; iVar < 8; iVar++) nodalDisplacement[iVar] = new su2double[nDim];
	}


	/*--- If it is linear elasticity, D is constant along the calculations ---*/

	Compute_Constitutive_Matrix();

}
void CFEM_NonlinearElasticity::Compute_Tangent_Matrix(CElement *element, CConfig *config){

	unsigned short iVar, jVar, kVar;
	unsigned short iGauss, nGauss;
	unsigned short iNode, jNode, nNode;
	unsigned short iDim, bDim;

	su2double Ks_Aux_ab;

	su2double Weight, Jac_x;

	su2double AuxMatrixKc[3][6];
	su2double AuxMatrixKs[3];

	/*--- Initialize auxiliary matrices ---*/

	if (nDim == 2) bDim = 3;
	else bDim = 6;

	for (iVar = 0; iVar < bDim; iVar++){
		for (jVar = 0; jVar < nDim; jVar++){
			Ba_Mat[iVar][jVar] = 0.0;
			Bb_Mat[iVar][jVar] = 0.0;
		}
	}

	for (iVar = 0; iVar < 3; iVar++){
		for (jVar = 0; jVar < 6; jVar++){
			AuxMatrixKc[iVar][jVar] = 0.0;
		}
	}

	for (iVar = 0; iVar < 3; iVar++){
		AuxMatrixKs[iVar] = 0.0;
	}

	element->clearElement(); 			/*--- Restarts the element: avoids adding over previous results in other elements --*/
	element->ComputeGrad_NonLinear();

	nNode = element->GetnNodes();
	nGauss = element->GetnGaussPoints();

	/*--- Full integration of the constitutive and stress term ---*/

	for (iGauss = 0; iGauss < nGauss; iGauss++){

		Weight = element->GetWeight(iGauss);
		Jac_x = element->GetJ_x(iGauss);

		/*--- Initialize the deformation gradient for each Gauss Point ---*/

		for (iVar = 0; iVar < 3; iVar++){
			for (jVar = 0; jVar < 3; jVar++){
				F_Mat[iVar][jVar] = 0.0;
				b_Mat[iVar][jVar] = 0.0;
			}
		}

		/*--- Retrieve the values of the gradients of the shape functions for each node ---*/
		/*--- This avoids repeated operations ---*/

		for (iNode = 0; iNode < nNode; iNode++){

			for (iDim = 0; iDim < nDim; iDim++){
				GradNi_Ref_Mat[iNode][iDim] = element->GetGradNi_X(iNode,iGauss,iDim);
				GradNi_Curr_Mat[iNode][iDim] = element->GetGradNi_x(iNode,iGauss,iDim);
				currentCoord[iNode][iDim] = element->GetCurr_Coord(iNode, iDim);
			}

			/*--- Compute the deformation gradient ---*/

			for (iVar = 0; iVar < nDim; iVar++){
				for (jVar = 0; jVar < nDim; jVar++){
					F_Mat[iVar][jVar] += currentCoord[iNode][iVar]*GradNi_Ref_Mat[iNode][jVar];
				}
			}

			/*--- This implies plane strain --> Consider the possible implementation for plane stress --*/
			if (nDim == 2){
				F_Mat[2][2] = 1.0;
			}

		}

		if (nDim == 2) {
			if (plane_stress){
				// Compute the value of the term 33 for the deformation gradient
				Compute_Plane_Stress_Term(element, config);
				F_Mat[2][2] = f33;
			}
			else{
				F_Mat[2][2] = 1.0;
			}
		}

		/*--- Determinant of F --> Jacobian of the transformation ---*/

		J_F = 	F_Mat[0][0]*F_Mat[1][1]*F_Mat[2][2]+
				F_Mat[0][1]*F_Mat[1][2]*F_Mat[2][0]+
				F_Mat[0][2]*F_Mat[1][0]*F_Mat[2][1]-
				F_Mat[0][2]*F_Mat[1][1]*F_Mat[2][0]-
				F_Mat[1][2]*F_Mat[2][1]*F_Mat[0][0]-
				F_Mat[2][2]*F_Mat[0][1]*F_Mat[1][0];

		/*--- Compute the left Cauchy deformation tensor ---*/

		for (iVar = 0; iVar < 3; iVar++){
			for (jVar = 0; jVar < 3; jVar++){
				for (kVar = 0; kVar < 3; kVar++){
					b_Mat[iVar][jVar] += F_Mat[iVar][kVar]*F_Mat[jVar][kVar];
				}
			}
		}

		/*--- Compute the constitutive matrix ---*/

		Compute_Constitutive_Matrix(element, config);
		Compute_Stress_Tensor(element, config);


		for (iNode = 0; iNode < nNode; iNode++){

			/*--------------------------------------------------------------------------------*/
			/*---------------------------- NODAL STRESS TERM ---------------------------------*/
			/*--------------------------------------------------------------------------------*/
		    /*--- Compute the nodal stress term for each gaussian point and for each node, ---*/
		    /*--- and add it to the element structure to be retrieved from the solver      ---*/

			for (iVar = 0; iVar < nDim; iVar++){
				KAux_t_a[iVar] = 0.0;
				for (jVar = 0; jVar < nDim; jVar++){
					KAux_t_a[iVar] += Weight * Stress_Tensor[iVar][jVar] * GradNi_Curr_Mat[iNode][jVar] * Jac_x;
				}
			}

			element->Add_Kt_a(KAux_t_a, iNode);

			/*--------------------------------------------------------------------------------*/
			/*----------------------- CONSTITUTIVE AND STRESS TERM ---------------------------*/
			/*--------------------------------------------------------------------------------*/

			if (nDim == 2){
				Ba_Mat[0][0] = GradNi_Curr_Mat[iNode][0];
				Ba_Mat[1][1] = GradNi_Curr_Mat[iNode][1];
				Ba_Mat[2][0] = GradNi_Curr_Mat[iNode][1];
				Ba_Mat[2][1] = GradNi_Curr_Mat[iNode][0];
			}
			else if (nDim ==3){
				Ba_Mat[0][0] = GradNi_Curr_Mat[iNode][0];
				Ba_Mat[1][1] = GradNi_Curr_Mat[iNode][1];
				Ba_Mat[2][2] = GradNi_Curr_Mat[iNode][2];
				Ba_Mat[3][0] = GradNi_Curr_Mat[iNode][1];
				Ba_Mat[3][1] = GradNi_Curr_Mat[iNode][0];
				Ba_Mat[4][0] = GradNi_Curr_Mat[iNode][2];
				Ba_Mat[4][2] = GradNi_Curr_Mat[iNode][0];
				Ba_Mat[5][1] = GradNi_Curr_Mat[iNode][2];
				Ba_Mat[5][2] = GradNi_Curr_Mat[iNode][1];
			}

		    /*--- Compute the BT.D Matrix ---*/

			for (iVar = 0; iVar < nDim; iVar++){
				for (jVar = 0; jVar < bDim; jVar++){
					AuxMatrixKc[iVar][jVar] = 0.0;
					for (kVar = 0; kVar < bDim; kVar++){
						AuxMatrixKc[iVar][jVar] += Ba_Mat[kVar][iVar]*D_Mat[kVar][jVar];
					}
				}
			}

		    /*--- Compute the BT.D Matrix ---*/

			for (iVar = 0; iVar < nDim; iVar++){
				AuxMatrixKs[iVar] = 0.0;
				for (jVar = 0; jVar < nDim; jVar++){
					AuxMatrixKs[iVar] += GradNi_Curr_Mat[iNode][jVar]*Stress_Tensor[jVar][iVar]; // DOUBLE CHECK
				}
			}

			/*--- Assumming symmetry ---*/
			for (jNode = iNode; jNode < nNode; jNode++){
				if (nDim == 2){
					Bb_Mat[0][0] = GradNi_Curr_Mat[jNode][0];
					Bb_Mat[1][1] = GradNi_Curr_Mat[jNode][1];
					Bb_Mat[2][0] = GradNi_Curr_Mat[jNode][1];
					Bb_Mat[2][1] = GradNi_Curr_Mat[jNode][0];
				}
				else if (nDim ==3){
					Bb_Mat[0][0] = GradNi_Curr_Mat[jNode][0];
					Bb_Mat[1][1] = GradNi_Curr_Mat[jNode][1];
					Bb_Mat[2][2] = GradNi_Curr_Mat[jNode][2];
					Bb_Mat[3][0] = GradNi_Curr_Mat[jNode][1];
					Bb_Mat[3][1] = GradNi_Curr_Mat[jNode][0];
					Bb_Mat[4][0] = GradNi_Curr_Mat[jNode][2];
					Bb_Mat[4][2] = GradNi_Curr_Mat[jNode][0];
					Bb_Mat[5][1] = GradNi_Curr_Mat[jNode][2];
					Bb_Mat[5][2] = GradNi_Curr_Mat[jNode][1];
				}

				/*--- KAux_ab is the term for the constitutive part of the tangent matrix ---*/
				for (iVar = 0; iVar < nDim; iVar++){
					for (jVar = 0; jVar < nDim; jVar++){
						KAux_ab[iVar][jVar] = 0.0;
						for (kVar = 0; kVar < bDim; kVar++){
							KAux_ab[iVar][jVar] += Weight * AuxMatrixKc[iVar][kVar] * Bb_Mat[kVar][jVar] * Jac_x;
						}
					}
				}

				/*--- Ks_Aux_ab is the term for the constitutive part of the tangent matrix ---*/
				Ks_Aux_ab = 0.0;
				for (iVar = 0; iVar < nDim; iVar++){
					Ks_Aux_ab += Weight * AuxMatrixKs[iVar] * GradNi_Curr_Mat[jNode][iVar] * Jac_x;
				}

				element->Add_Kab(KAux_ab,iNode, jNode);
				element->Add_Ks_ab(Ks_Aux_ab,iNode, jNode);
				/*--- Symmetric terms --*/
				if (iNode != jNode){
					element->Add_Kab_T(KAux_ab, jNode, iNode);
					element->Add_Ks_ab(Ks_Aux_ab,jNode, iNode);
				}

			}

		}

	}

}
void CFEALinearElasticity::Compute_Tangent_Matrix(CElement *element, CConfig *config) {

  unsigned short iVar, jVar, kVar;
  unsigned short iGauss, nGauss;
  unsigned short iNode, jNode, nNode;
  unsigned short iDim;
  unsigned short bDim;

  su2double Weight, Jac_X;

  su2double AuxMatrix[3][6], *res_aux = new su2double[nVar];
  
  /*--- Set element properties and recompute the constitutive matrix, this is needed
        for multiple material cases and for correct differentiation ---*/
  SetElement_Properties(element, config);
  
  /*--- Register pre-accumulation inputs, material props and nodal coords ---*/
  AD::StartPreacc();
  AD::SetPreaccIn(E);
  AD::SetPreaccIn(Nu);
  AD::SetPreaccIn(Rho_s);
  AD::SetPreaccIn(Rho_s_DL);
  element->SetPreaccIn_Coords();
  /*--- Recompute Lame parameters as they depend on the material properties ---*/
  Compute_Lame_Parameters();
  
  Compute_Constitutive_Matrix(element, config);

  /*--- Initialize auxiliary matrices ---*/

  if (nDim == 2) bDim = 3;
  else bDim = 6;

  for (iVar = 0; iVar < bDim; iVar++) {
    for (jVar = 0; jVar < nDim; jVar++) {
      Ba_Mat[iVar][jVar] = 0.0;
      Bb_Mat[iVar][jVar] = 0.0;
    }
  }

  for (iVar = 0; iVar < 3; iVar++) {
    for (jVar = 0; jVar < 6; jVar++) {
      AuxMatrix[iVar][jVar] = 0.0;
    }
  }

  element->clearElement();       /*--- Restarts the element: avoids adding over previous results in other elements --*/
  element->ComputeGrad_Linear();
  nNode = element->GetnNodes();
  nGauss = element->GetnGaussPoints();

  for (iGauss = 0; iGauss < nGauss; iGauss++) {

    Weight = element->GetWeight(iGauss);
    Jac_X = element->GetJ_X(iGauss);

    /*--- Retrieve the values of the gradients of the shape functions for each node ---*/
    /*--- This avoids repeated operations ---*/
    for (iNode = 0; iNode < nNode; iNode++) {
      for (iDim = 0; iDim < nDim; iDim++) {
        GradNi_Ref_Mat[iNode][iDim] = element->GetGradNi_X(iNode,iGauss,iDim);
      }
    }

    for (iNode = 0; iNode < nNode; iNode++) {

      if (nDim == 2) {
        Ba_Mat[0][0] = GradNi_Ref_Mat[iNode][0];
        Ba_Mat[1][1] = GradNi_Ref_Mat[iNode][1];
        Ba_Mat[2][0] = GradNi_Ref_Mat[iNode][1];
        Ba_Mat[2][1] = GradNi_Ref_Mat[iNode][0];
      }
      else if (nDim == 3) {
        Ba_Mat[0][0] = GradNi_Ref_Mat[iNode][0];
        Ba_Mat[1][1] = GradNi_Ref_Mat[iNode][1];
        Ba_Mat[2][2] = GradNi_Ref_Mat[iNode][2];
        Ba_Mat[3][0] = GradNi_Ref_Mat[iNode][1];
        Ba_Mat[3][1] = GradNi_Ref_Mat[iNode][0];
        Ba_Mat[4][0] = GradNi_Ref_Mat[iNode][2];
        Ba_Mat[4][2] = GradNi_Ref_Mat[iNode][0];
        Ba_Mat[5][1] = GradNi_Ref_Mat[iNode][2];
        Ba_Mat[5][2] = GradNi_Ref_Mat[iNode][1];
      }

        /*--- Compute the BT.D Matrix ---*/

      for (iVar = 0; iVar < nDim; iVar++) {
        for (jVar = 0; jVar < bDim; jVar++) {
          AuxMatrix[iVar][jVar] = 0.0;
          for (kVar = 0; kVar < bDim; kVar++) {
            AuxMatrix[iVar][jVar] += Ba_Mat[kVar][iVar]*D_Mat[kVar][jVar];
          }
        }
      }

      /*--- Assumming symmetry ---*/
      for (jNode = iNode; jNode < nNode; jNode++) {
        if (nDim == 2) {
          Bb_Mat[0][0] = GradNi_Ref_Mat[jNode][0];
          Bb_Mat[1][1] = GradNi_Ref_Mat[jNode][1];
          Bb_Mat[2][0] = GradNi_Ref_Mat[jNode][1];
          Bb_Mat[2][1] = GradNi_Ref_Mat[jNode][0];
        }
        else if (nDim ==3) {
          Bb_Mat[0][0] = GradNi_Ref_Mat[jNode][0];
          Bb_Mat[1][1] = GradNi_Ref_Mat[jNode][1];
          Bb_Mat[2][2] = GradNi_Ref_Mat[jNode][2];
          Bb_Mat[3][0] = GradNi_Ref_Mat[jNode][1];
          Bb_Mat[3][1] = GradNi_Ref_Mat[jNode][0];
          Bb_Mat[4][0] = GradNi_Ref_Mat[jNode][2];
          Bb_Mat[4][2] = GradNi_Ref_Mat[jNode][0];
          Bb_Mat[5][1] = GradNi_Ref_Mat[jNode][2];
          Bb_Mat[5][2] = GradNi_Ref_Mat[jNode][1];
        }

        for (iVar = 0; iVar < nDim; iVar++) {
          for (jVar = 0; jVar < nDim; jVar++) {
            KAux_ab[iVar][jVar] = 0.0;
            for (kVar = 0; kVar < bDim; kVar++) {
              KAux_ab[iVar][jVar] += Weight * AuxMatrix[iVar][kVar] * Bb_Mat[kVar][jVar] * Jac_X;
            }
          }
        }

        element->Add_Kab(KAux_ab,iNode, jNode);
        /*--- Symmetric terms --*/
        if (iNode != jNode) {
          element->Add_Kab_T(KAux_ab, jNode, iNode);
        }

      }

    }

  }
  
  // compute residual
  for(iNode = 0; iNode<nNode; ++iNode)
  {
    for(jNode = 0; jNode<nNode; ++jNode)
    {
      su2double *Kab = element->Get_Kab(iNode,jNode);
      
      for (iVar = 0; iVar < nVar; iVar++) {
          res_aux[iVar] = 0.0;
          for (jVar = 0; jVar < nVar; jVar++)
            res_aux[iVar] += Kab[iVar*nVar+jVar]*
              (element->GetCurr_Coord(jNode,jVar)-element->GetRef_Coord(jNode,jVar));
      }
      element->Add_Kt_a(res_aux,iNode);
    }
  }
  
  /*--- Register the stress residual as preaccumulation output ---*/
  element->SetPreaccOut_Kt_a();
  AD::EndPreacc();
  
  delete[] res_aux;
}
void CFEALinearElasticity::Compute_Averaged_NodalStress(CElement *element, CConfig *config) {

  unsigned short iVar, jVar;
  unsigned short iGauss, nGauss;
  unsigned short iNode, nNode;
  unsigned short iDim, bDim;

  /*--- Auxiliary vector ---*/
  su2double Strain[6], Stress[6];
  
  /*--- Set element properties and recompute the constitutive matrix, this is needed
        for multiple material cases and for correct differentiation ---*/
  SetElement_Properties(element, config);
  Compute_Constitutive_Matrix(element, config);

  /*--- Initialize auxiliary matrices ---*/

  if (nDim == 2) bDim = 3;
  else bDim = 6;

  for (iVar = 0; iVar < bDim; iVar++) {
    for (jVar = 0; jVar < nDim; jVar++) {
      Ba_Mat[iVar][jVar] = 0.0;
    }
  }

  element->clearStress();       /*--- Clears the stress in the element: avoids adding over previous results in other elements --*/
  element->ComputeGrad_Linear();
  nNode = element->GetnNodes();
  nGauss = element->GetnGaussPoints();

  for (iGauss = 0; iGauss < nGauss; iGauss++) {

    /*--- Retrieve the values of the gradients of the shape functions for each node ---*/
    /*--- This avoids repeated operations ---*/
    for (iNode = 0; iNode < nNode; iNode++) {
      for (iDim = 0; iDim < nDim; iDim++) {
        GradNi_Ref_Mat[iNode][iDim] = element->GetGradNi_X(iNode,iGauss,iDim);
        nodalDisplacement[iNode][iDim] = element->GetCurr_Coord(iNode, iDim) - element->GetRef_Coord(iNode, iDim);
      }
    }

    for (iVar = 0; iVar < bDim; iVar++) {
      Strain[iVar] = 0.0;
    }

    for (iNode = 0; iNode < nNode; iNode++) {

      /*--- Set matrix B ---*/
      if (nDim == 2) {
        Ba_Mat[0][0] = GradNi_Ref_Mat[iNode][0];
        Ba_Mat[1][1] = GradNi_Ref_Mat[iNode][1];
        Ba_Mat[2][0] = GradNi_Ref_Mat[iNode][1];
        Ba_Mat[2][1] = GradNi_Ref_Mat[iNode][0];
      }
      else if (nDim ==3) {
        Ba_Mat[0][0] = GradNi_Ref_Mat[iNode][0];
        Ba_Mat[1][1] = GradNi_Ref_Mat[iNode][1];
        Ba_Mat[2][2] = GradNi_Ref_Mat[iNode][2];
        Ba_Mat[3][0] = GradNi_Ref_Mat[iNode][1];
        Ba_Mat[3][1] = GradNi_Ref_Mat[iNode][0];
        Ba_Mat[4][0] = GradNi_Ref_Mat[iNode][2];
        Ba_Mat[4][2] = GradNi_Ref_Mat[iNode][0];
        Ba_Mat[5][1] = GradNi_Ref_Mat[iNode][2];
        Ba_Mat[5][2] = GradNi_Ref_Mat[iNode][1];
      }

        /*--- Compute the Strain Vector as B*u ---*/

      for (iVar = 0; iVar < bDim; iVar++) {
        for (jVar = 0; jVar < nDim; jVar++) {
          Strain[iVar] += Ba_Mat[iVar][jVar]*nodalDisplacement[iNode][jVar];
        }
      }

    }

      /*--- Compute the Stress Vector as D*epsilon ---*/

    for (iVar = 0; iVar < bDim; iVar++) {
      Stress[iVar] = 0.0;
      for (jVar = 0; jVar < bDim; jVar++) {
        Stress[iVar] += D_Mat[iVar][jVar]*Strain[jVar];
      }
    }

    for (iNode = 0; iNode < nNode; iNode++) {
      /*--- If nDim is 3 and we compute it this way, the 3rd component is the Szz, while in the ---*/
      /*--- output it is the 4th component for practical reasons ---*/
      if (nDim == 2) {
        element->Add_NodalStress(Stress[0] * element->GetNi_Extrap(iNode, iGauss), iNode, 0);
        element->Add_NodalStress(Stress[1] * element->GetNi_Extrap(iNode, iGauss), iNode, 1);
        element->Add_NodalStress(Stress[2] * element->GetNi_Extrap(iNode, iGauss), iNode, 2);
      }
      else if (nDim == 3) {
        element->Add_NodalStress(Stress[0] * element->GetNi_Extrap(iNode, iGauss), iNode, 0);
        element->Add_NodalStress(Stress[1] * element->GetNi_Extrap(iNode, iGauss), iNode, 1);
        element->Add_NodalStress(Stress[3] * element->GetNi_Extrap(iNode, iGauss), iNode, 2);
        element->Add_NodalStress(Stress[2] * element->GetNi_Extrap(iNode, iGauss), iNode, 3);
        element->Add_NodalStress(Stress[4] * element->GetNi_Extrap(iNode, iGauss), iNode, 4);
        element->Add_NodalStress(Stress[5] * element->GetNi_Extrap(iNode, iGauss), iNode, 5);
      }
    }



  }


}