//
// Created by Morten Nobel and Niels Aage 11/10/16.
//
#include <cassert>
#include <petscdmplex.h>
#include <iostream>
#include <vector>
#include <set>
#include <sstream>
#include <iomanip>
#include <algorithm>
#include <sstream>
#include <petscdmplex.h>
#include "exodusii-custom.h"
#include <petsc.h>
#include "petscsf.h"


using namespace std;

namespace {
  int reorder[] = {4,7,3,0,5,1,2,6};
}

PetscInt Hex8Isoparametric(PetscScalar *X, PetscScalar *Y, PetscScalar *Z, PetscScalar nu, PetscInt redInt, PetscScalar *ke);
PetscScalar Dot(PetscScalar *v1, PetscScalar *v2, PetscInt l);
void DifferentiatedShapeFunctions(PetscScalar xi, PetscScalar eta, PetscScalar zeta, PetscScalar *dNdxi, PetscScalar *dNdeta, PetscScalar *dNdzeta);
PetscScalar Inverse3M(PetscScalar J[][3], PetscScalar invJ[][3]);
int GetLocalDOF(DM dm, PetscInt point);

DM LoadDMPlex(const char * filename, bool distribute = true){
  DM dm;
  PetscBool interpolate = PETSC_TRUE;
  
  int rank;
  MPI_Comm_rank (PETSC_COMM_WORLD, &rank);	// get current process id
  PetscInt faceSets;
  CDMPlexCreateExodusFromFile(PETSC_COMM_WORLD, filename, interpolate, &dm, &faceSets);
  //CHKERRQ(ierr);
  
  if (distribute){
    DM dmDist;
    // Distribute mesh over processes
    //DMPlexSetAdjacencyUseCone(dm,PETSC_FALSE);
    //DMPlexSetAdjacencyUseClosure(dm,PETSC_TRUE);
    DMPlexDistribute(dm, 0, NULL, &dmDist);
    //assert("DMPlexDistribute",rank>0 || dmDist != nullptr);
    if (dmDist) {
      DMDestroy(&dm);
      dm = dmDist;
    } else{
      cerr << "Not partitioned"<< endl;
    }
  }
  return dm;
}

bool isGhost(DM dm, PetscInt point){
  PetscSection globalSection;
  DMGetDefaultGlobalSection(dm, &globalSection);
  PetscInt offset;
  PetscSectionGetOffset(globalSection,point, &offset);
  return offset < 0;
}

int getLabelValue(DM dm, char* name, PetscInt point){
  PetscInt value;
  
  DMGetLabelValue(dm, name, point,&value);
  
  return value;
}

/* MGK: Remove this function and data structure */
vector<vector<PetscInt>> createTopologyMatrix(DM dm){
  vector<vector<PetscInt>> res;
  
  int rank;
  MPI_Comm_rank (PETSC_COMM_WORLD, &rank);	// get current process id
  
  PetscSynchronizedPrintf(PETSC_COMM_WORLD,"Hello 1 \n");
  PetscSynchronizedFlush(PETSC_COMM_WORLD,PETSC_STDOUT);
  
  PetscInt cStart, cEnd;
  DMPlexGetHeightStratum(dm, 0, &cStart, &cEnd);
  
  PetscInt vStart, vEnd;
  DMPlexGetHeightStratum(dm, 3, &vStart, &vEnd);
  
  // Get coordinates
  PetscSection coordSection;
  Vec coordinates;
  PetscScalar *coords;
  PetscBool local = PETSC_TRUE;
  
  DMGetCoordinateSection(dm, &coordSection);
  if (local){
    DMGetCoordinatesLocal(dm, &coordinates);
  } else {
    DMGetCoordinates(dm, &coordinates);
  }
  VecGetArray(coordinates, &coords);
  
  // Total number of elements
  PetscInt start, slut;
  DMPlexGetHeightStratum(dm, 0, &start, &slut);
  start = slut-start;
  PetscInt tmp = 0;
  MPI_Allreduce(&start, &tmp, 1,MPIU_INT, MPI_SUM,PETSC_COMM_WORLD );
  PetscSynchronizedPrintf(PETSC_COMM_WORLD,"Total number of elements: %d (from %d to %d)\n", tmp, cStart, cEnd);
  PetscSynchronizedFlush(PETSC_COMM_WORLD,PETSC_STDOUT);
  
  PetscSynchronizedPrintf(PETSC_COMM_WORLD,"Hello 2 \n");
  PetscSynchronizedFlush(PETSC_COMM_WORLD,PETSC_STDOUT);
  
  res.resize(cStart, vector<PetscInt>()); // this should not do anything in the way PETSc layout the hasse diagram
  
  PetscSynchronizedPrintf(PETSC_COMM_WORLD,"Rank %d iterate from %d to %d\n",rank,cStart, cEnd);
  PetscSynchronizedFlush(PETSC_COMM_WORLD,PETSC_STDOUT);
  
  for (PetscInt c = cStart; c < cEnd; ++c) {

#ifdef MGK
    /* It looks like you can replace everything below with one call */
    PetscScalar *coords = NULL;
    PetscInt     numCoords;

    ierr = DMPlexVecGetClosure(dm, coordSection, coordinates, c, &numCoords, &coords);CHKERRQ(ierr);
#else
    PetscInt clojureCount;
    PetscInt *clojureIndex = nullptr;
    vector<PetscInt> vertices(8);
    // find all child points
    auto useCone = PETSC_TRUE;
    DMPlexGetTransitiveClosure(dm, c, useCone, &clojureCount, &clojureIndex);
    //PetscSynchronizedPrintf(PETSC_COMM_WORLD,"Rank %d New New  clojureCount %d \n", rank, clojureCount);
    //PetscSynchronizedFlush(PETSC_COMM_WORLD,PETSC_STDOUT);
    int count = 0;
    //PetscSynchronizedPrintf(PETSC_COMM_WORLD,"Rank %d Elem(local) %d clojure %d\n",rank, c,clojureCount);
    for (int v = 0;v<clojureCount;v++) {
      // if not found in out edges then point must be a vertex
      PetscInt cPoint = clojureIndex[v*2];
      PetscInt cOrientation = clojureIndex[v*2+1];
      bool isVertex = cPoint >= vStart && cPoint < vEnd; // is vertex if it exist in the vertex range
      if (isVertex) {
	assert (cPoint>=0);
	vertices[reorder[count]] = cPoint;
	count++;
      }
    }

    PetscSection globalSection;
    DMGetDefaultGlobalSection(dm, &globalSection);
    PetscInt offset;
    
    //PetscSynchronizedPrintf(PETSC_COMM_WORLD,"\n");
    for (int i=0;i<8;i++){
      PetscSectionGetOffset(globalSection, vertices[i], &offset);
      
      // auto pos = coords.getCoordinate(vertices[i]);
      PetscScalar* pos;
      VecGetValuesSection(coordinates, coordSection,vertices[i],&pos);
      
      //PetscSynchronizedPrintf(PETSC_COMM_WORLD,"Rank %d id %d pos %f %f %f global offset %d\n",rank, vertices[i], pos[0], pos[1], pos[2], offset);
    }
    DMPlexRestoreTransitiveClosure(dm, c, useCone, &clojureCount, &clojureIndex);
    
    // Clean up
    VecRestoreArray(coordinates, &coords);
#endif
    
    // Get 
    Vec coordinatesLoc;
    DMGetCoordinatesLocal(dm, &coordinatesLoc);
    
    ISLocalToGlobalMapping  map;
    VecGetLocalToGlobalMapping(coordinatesLoc,&map);
    
    PetscSection coordSectionLoc;
    DMGetCoordinateSection(dm, &coordSectionLoc);
    
    res.push_back(vertices);
    
  }
  PetscSynchronizedPrintf(PETSC_COMM_WORLD,"Hello 3 \n");
  PetscSynchronizedFlush(PETSC_COMM_WORLD,PETSC_STDOUT);
  return res;
}

void print(const char *str){
  PetscSynchronizedPrintf(PETSC_COMM_WORLD,str);
  PetscSynchronizedFlush(PETSC_COMM_WORLD,PETSC_STDOUT);
}


// height : 0 = element, 1 = face, 2 = edge, 3 = vertex
void SetVectorBasedOnLabel(DM dm, PetscScalar* loadValue, const char* labelName, PetscInt labelValue, bool duplicate, int height, Vec TMP, PetscScalar defaultValue){
  PetscSection localSection;
  DMGetDefaultSection(dm, &localSection);
  Vec loc;
  DMGetLocalVector(dm, &loc);
  VecSet(loc,defaultValue);
  
  PetscInt start, end;
  DMPlexGetHeightStratum(dm, height, &start, &end);
  
  PetscInt vStart, vEnd;
  DMPlexGetHeightStratum(dm, 3, &vStart, &vEnd);
  
  int foundFaces = 0;
  int changedDofs = 0;
  
  for (PetscInt point = start;point < end; point++){
    PetscInt thisLabelValue = -1;
    DMGetLabelValue(dm, labelName, point,&thisLabelValue);
    
    if (thisLabelValue == labelValue){
      foundFaces++;
      PetscInt clojureCount;
      PetscInt *clojureIndex = nullptr;
      vector<PetscInt> vertices;
      // find all child points
      auto useCone = PETSC_TRUE;
      DMPlexGetTransitiveClosure(dm, point, useCone, &clojureCount, &clojureIndex);
      for (int v = 0;v<clojureCount;v++) {
	PetscInt cPoint = clojureIndex[v*2];
	PetscInt cOrientation = clojureIndex[v*2+1];
	bool isVertex = cPoint >= vStart && cPoint < vEnd; // is vertex if it exist in the vertex range
	if (isVertex) {
	  VecSetValuesSection(loc, localSection,cPoint, loadValue, INSERT_VALUES);
	  changedDofs++;
	}
      }
      DMPlexRestoreTransitiveClosure(dm, point, useCone, &clojureCount, &clojureIndex);
    }
  }
  VecAssemblyBegin(loc);
  VecAssemblyEnd(loc);
  
  DMLocalToGlobalBegin(dm,loc,INSERT_VALUES,TMP);
  DMLocalToGlobalEnd(dm,loc,INSERT_VALUES,TMP);
  
  DMRestoreLocalVector(dm, &loc);
}

/*
MGK:
1) None of the 0 dof sets are necessary

2) This is just P1, so you could use PetscFE to do it
*/
PetscSection setupDOFs(DM dm) {
  PetscSection s;
  
  PetscSectionCreate(PETSC_COMM_WORLD, &s);
  
  
  PetscSectionSetNumFields(s, 1);
  
  PetscInt pStart, pEnd, cStart, cEnd, fStart, fEnd, vStart, vEnd, eStart, eEnd;
  DMPlexGetChart(dm, &pStart, &pEnd);
  DMPlexGetHeightStratum(dm, 0, &cStart, &cEnd);
  DMPlexGetHeightStratum(dm, 1, &fStart, &fEnd);
  DMPlexGetHeightStratum(dm, 2, &eStart, &eEnd);
  DMPlexGetDepthStratum(dm, 0, &vStart, &vEnd);
  PetscSectionSetChart(s, pStart, pEnd);
  
  // set dofs for each point in hasse diagram
  for (PetscInt c = cStart; c < cEnd; ++c) {
    PetscSectionSetDof(s, c, 0);
    PetscSectionSetFieldDof(s, c, 0, 0);
  }
  
  // Set dof only needed for vertices
  for (PetscInt f = fStart; f < fEnd; ++f) {
    PetscSectionSetDof(s, f, 0);
    PetscSectionSetFieldDof(s, f, 0, 0);
  }
  
  for (PetscInt e = eStart; e < eEnd; ++e) {
    PetscSectionSetDof(s, e, 0);
    PetscSectionSetFieldDof(s, e, 0, 0);
  }
  
  for (PetscInt  v = vStart; v < vEnd; ++v) {
    PetscSectionSetDof(s, v, 3);
    PetscSectionSetFieldDof(s, v, 0, 3);
  }
  
  PetscSectionSetUp(s);
  DMSetDefaultSection(dm, s);
  
  return s;
}

/* MGK: Function is unused */
// forceGlobalIndex translate negative (non-local) values into actual global values
int localToGlobal(DM dm, PetscInt point, bool forceGlobalIndex=true){
  PetscSection globalSection;
  DMGetDefaultGlobalSection(dm, &globalSection);
  PetscInt offset;
  PetscSectionGetOffset(globalSection, point, &offset);
  if (forceGlobalIndex && offset < 0){
    offset = -offset +1;
  }
  return offset;
}

//
// Returns the global offset
//
//
PetscInt GetLocalDOF(DM dm, PetscInt point){
  PetscSection localSection;
  DMGetDefaultSection(dm, &localSection);
  PetscInt offset;
  PetscSectionGetOffset(localSection, point, &offset);
  assert(offset >=0);
  return offset;
}

void CreateGlobalStiffnessMatrix(DM dm, vector<vector<PetscInt>> topMatrix, Mat &K){
  
  int rank;
  PetscErrorCode ierr;
  MPI_Comm_rank(PETSC_COMM_WORLD, &rank);	// get current process id
  
  // Allocate the system matrix
  DMCreateMatrix(dm,&K);
  
  MatZeroEntries(K);
  // Number of local elements
  PetscInt nel = topMatrix.size(); /* MGK: This is unsafe. You should use GetHeightStratum() */
  PetscSynchronizedPrintf(PETSC_COMM_WORLD,"Rank: %d, nel %d\n ",rank,nel);
  PetscSynchronizedFlush(PETSC_COMM_WORLD,PETSC_STDOUT);
  
  // Get coordinates
  PetscSection coordSection;
  Vec coordinates;
  PetscScalar *coords;
  PetscBool local = PETSC_TRUE;
  
  DMGetCoordinateSection(dm, &coordSection);
  if (local){
    DMGetCoordinatesLocal(dm, &coordinates);
  } else {
    DMGetCoordinates(dm, &coordinates);
  }
  VecGetArray(coordinates, &coords);
  
  // Edof vector
  PetscInt edof[24];
  
  // element matrix and nu
  PetscScalar ke[24*24], nu = 0.3;
  
  for (PetscInt e = 0; e < 24*24; e++ ){
    ke[e] = 0.0;
  }
  
  // Element loop
  for (PetscInt e = 0; e < nel; e++ ){
    // Get element connectivity reference from topMatrix[e]
    vector<PetscInt>& econ = topMatrix[e];
    
#ifdef MGK
    /* The original code is crazy. Replace with the call below */
    PetscScalar *coords = NULL;
    PetscInt     numCoords;

    ierr = DMPlexVecGetClosure(dm, coordSection, coordinates, c, &numCoords, &coords);CHKERRQ(ierr);
#else
    // Get nodes
    PetscInt nen = econ.size();
    //printf("number of element nodes: %d \n",nen);
    
    // Get Coordinates
    PetscScalar *xyz, X[8],Y[8],Z[8];
    
    // Loop over nodes
    for (PetscInt n = 0; n < nen; n++ ){
      //printf("%d ",econ[n]);
      PetscScalar* xyz;
      VecGetValuesSection(coordinates, coordSection,econ[n],&xyz);
      //xyz = coords.getCoordinate(econ[n]);
      X[n] = xyz[0];
      Y[n] = xyz[1];
      Z[n] = xyz[2];
      //printf("Rank: %d - Coords: (%e, %e, %e) \n ",rank,X[n],Y[n],Z[n]);
    }
    // printf("\n");
#endif
    
    // Get coordinates
    // THE GOAL TO CALL
    PetscInt redInt = 0;
    Hex8Isoparametric(X, Y, Z, nu, redInt, ke);

#ifdef MGK
    /* MGK You do not need any of the code below, and its error prone */
    ierr = DMPlexMatSetClosure(dm, NULL, NULL, K, e, ke, ADD_VALUES);CHKERRQ(ierr);
#else    
    PetscSection localSection;
    DMGetDefaultSection(dm, &localSection);
    
    // Get local dofs - edof
    // Node in element loop
    for (PetscInt n = 0; n < nen; n++ ){
      PetscInt dof = GetLocalDOF(dm,econ[n]);
      PetscInt dofSize;
      PetscSectionGetDof(localSection, econ[n], &dofSize);
      for (PetscInt i=0;i<dofSize;i++){
	int dofIndex = dof + i;
	edof[n*dofSize+i] = dofIndex;
      }
    }
    for (PetscInt k=0;k<24;k++){
      //printf("ID: %i, edof: %i\n",e,edof[k]);
    }
    //PetscSynchronizedFlush(PETSC_COMM_WORLD,PETSC_STDOUT);
    
    // Add to system matrix
    MatSetValuesLocal(K,24,edof,24,edof,ke,ADD_VALUES);
#endif
  }
  
  MatAssemblyBegin(K, MAT_FINAL_ASSEMBLY);
  MatAssemblyEnd(K, MAT_FINAL_ASSEMBLY);
  
}

void ComputeSomething(DM dm, vector<vector<PetscInt>> topMatrix, Vec &U){
  
  int rank;
  PetscErrorCode ierr;
  MPI_Comm_rank (PETSC_COMM_WORLD, &rank);	// get current process id
   
  // Number of local elements
  PetscInt nel = topMatrix.size();
  PetscSynchronizedPrintf(PETSC_COMM_WORLD,"Rank: %d, nel %d\n ",rank,nel);
  PetscSynchronizedFlush(PETSC_COMM_WORLD,PETSC_STDOUT);
  
  // printf("number of elements: %d \n",nel);
  
  PetscSection coordSection;
  Vec coordinates;
  PetscScalar *coords;
  PetscBool local = PETSC_TRUE;
  
  DMGetCoordinateSection(dm, &coordSection);
  if (local){
    DMGetCoordinatesLocal(dm, &coordinates);
  } else {
    DMGetCoordinates(dm, &coordinates);
  }
  VecGetArray(coordinates, &coords);
  
  // Edof vector
  PetscInt edof[24];
  
  // Get Solution
  Vec Uloc;
  DMCreateLocalVector(dm,&Uloc);
  DMGlobalToLocalBegin(dm,U,INSERT_VALUES,Uloc);
  DMGlobalToLocalEnd(dm,U,INSERT_VALUES,Uloc);
  
  // get pointer to local vector
  PetscScalar *up;
  VecGetArray(Uloc,&up);
  
  
  // element matrix and nu
  PetscScalar ke[24*24], nu = 0.3;
  
  for (PetscInt e = 0; e < 24*24; e++ ){
    ke[e] = 0.0;
  }
  
  // Zero compliance
  PetscScalar uKu=0.0;
  
  // Element loop
  for (PetscInt e = 0; e < nel; e++ ){
    // Get element connectivity reference from topMatrix[e]
    vector<PetscInt>& econ = topMatrix[e];
    
    // Get nodes
    PetscInt nen = econ.size();
    //printf("number of element nodes: %d \n",nen);
    
    // Get Coordinates
    PetscScalar *xyz, X[8],Y[8],Z[8];
    
    // Loop over nodes
    for (PetscInt n = 0; n < nen; n++ ){
      //printf("%d ",econ[n]);
      //xyz = coords.getCoordinate(econ[n]);
      PetscScalar* xyz;
      VecGetValuesSection(coordinates, coordSection,econ[n],&xyz);
      X[n] = xyz[0];
      Y[n] = xyz[1];
      Z[n] = xyz[2];
      //printf("Rank: %d - Coords: (%e, %e, %e) \n ",rank,X[n],Y[n],Z[n]);
    }
    // printf("\n");
    
    // Get coordinates
    // THE GOAL TO CALL
    PetscInt redInt = 0;
    Hex8Isoparametric(X, Y, Z, nu, redInt, ke);
    
    for (PetscInt i=0;i<24;i++){
      if (ke[i*24+i]<0.0){
	printf("WE HAVE A PROBLEM at e: %i \n",e);
	break;
      }	
    }
    
    
    PetscSection localSection;
    DMGetDefaultSection(dm, &localSection);
    
    // Get local dofs - edof
    // Node in element loop
    for (PetscInt n = 0; n < nen; n++ ){
      PetscInt dof = GetLocalDOF(dm,econ[n]);
      PetscInt dofSize;
      PetscSectionGetDof(localSection, econ[n], &dofSize);
      
      //printf("Rank: %d - dof: %d - size: %d) \n ",rank,dof, dofSize);
      for (PetscInt i=0;i<dofSize;i++){
	int dofIndex = dof + i;
	edof[n*dofSize+i] = dofIndex;
      }
    }
    for (PetscInt k=0;k<24;k++){
      for (PetscInt h=0;h<24;h++){
	uKu += up[edof[k]]*ke[k*24+h]*up[edof[h]];
	//printf("ID: %i, edof: %i\n",e,edof[k]);
      }
    }	
    //PetscSynchronizedFlush(PETSC_COMM_WORLD,PETSC_STDOUT);
    
  }
  
  PetscSynchronizedPrintf(PETSC_COMM_WORLD,"Compliance LOCAL: %e\n",uKu);
  PetscSynchronizedFlush(PETSC_COMM_WORLD,PETSC_STDOUT);
  
  // Communicate
  PetscScalar tmp;
  MPI_Allreduce(&uKu, &tmp, 1,MPIU_SCALAR, MPI_SUM,PETSC_COMM_WORLD );
  uKu = tmp;
  
  // Print result
  PetscPrintf(PETSC_COMM_WORLD, "Compliance: %e\n",uKu);
}

void SolveFEM(){
  int rank;
  PetscErrorCode ierr;
  MPI_Comm_rank (PETSC_COMM_WORLD, &rank);	// get current process id
  
  print("Load mesh\n");
  
  
  // Determine the right place to write the new restart files
  PetscBool flg;
  char filenameChar[PETSC_MAX_PATH_LEN];
  //std::string filenameMesh = "MeshExodusII/CylinderMesh.e";
  std::string filenameMesh = "MeshExodusII/Mesh_Brick_01.e";	
  PetscOptionsGetString(NULL,NULL,"-filenameMesh",filenameChar,sizeof(filenameChar),&flg);
  if (flg){
    filenameMesh = "";
    filenameMesh.append(filenameChar);
  }
  // Assumes file exist
  DM dm = LoadDMPlex(filenameMesh.c_str(), true);
  print("Loading of mesh complete\n");
  
  print("Refine mesh\n");
  DM refine;
  DMRefine(dm,PETSC_COMM_WORLD,&refine);
  if (refine){
    DMDestroy(&dm);
    dm = refine;
  }
  
  print("Setup dofs\n");
  setupDOFs(dm);
  
  PetscInt nsize, msize;
  print("Create topology matrix - E2N \n");
  auto topMatrix = createTopologyMatrix(dm);
  
  print("Assemble global stiffness matrix\n");
  Mat K;
  CreateGlobalStiffnessMatrix(dm,topMatrix, K);
  
  MatGetSize(K,&nsize,&msize);
  printf("Size K[n x m]: %i  \n",nsize,msize);
  
  // Very simple BC approach
  Vec N;
  DMCreateGlobalVector(dm, &N);
  VecGetSize(N,&nsize);
  // LOOP OVER FACES IN THE SET AND SET ALL DOFS PER VERTIX TO 0
  PetscScalar ones[3] = {0.0,0.0,0.0};
  PetscScalar defaultValue = 1.0;
  SetVectorBasedOnLabel(dm, ones, "Face Set 0",2000,false,1,N,defaultValue);
  printf("Size N: %i  \n",nsize);
  
  // Very simple loading
  Vec F; 
  DMCreateGlobalVector(dm, &F);
  VecGetSize(F,&nsize);
  // LOOP OVER FACES IN THE SET AND SET y DOFS PER VERTIX TO -1
  PetscScalar someLoad[3] = {0.0,-1.0,0.0}; // todo
  defaultValue = 0.0;
  SetVectorBasedOnLabel(dm, someLoad, "Face Set 1",3000,false,1, F, defaultValue);
  printf("Size F: %i  \n",nsize);
  
  {
    PetscScalar trace;
    MatGetTrace(K,&trace);
    PetscPrintf(PETSC_COMM_WORLD,"Trace of matrix No BC: %f\n",trace);
  }
  
  
  // Impose the dirichlet conditions, i.e. K = N'*K*N - (N-I)
  // 1.: K = N'*K*N
  MatDiagonalScale(K,N,N);
  // 2. Add ones, i.e. K = K + NI, NI = I - N
  Vec NI;
  VecDuplicate(N,&NI);
  VecSet(NI,1.0);
  VecAXPY(NI,-1.0,N);
  MatDiagonalSet(K,NI,ADD_VALUES);
  // Zero out possible loads in the RHS that coincide
  VecPointwiseMult(F,F,N);
  
  {
    PetscScalar trace;
    MatGetTrace(K,&trace);
    PetscPrintf(PETSC_COMM_WORLD,"Trace of matrix With BC: %f\n",trace);
  }
  
  // Solve N^TKNu = N^F
  Vec X;
  VecDuplicate(N,&X);
  
  KSP ksp;
  KSPCreate(PETSC_COMM_WORLD,&ksp);
  KSPSetOperators(ksp,K,K);
  KSPSetFromOptions(ksp);
  
  KSPType ksptype;
  KSPSetType(ksp,KSPFGMRES);
  KSPGetType(ksp,&ksptype);
  PC pc;
  PCType pctype;
  KSPGetPC(ksp,&pc);
  PCSetType(pc,PCJACOBI);
  //PCSetType(pc,PCLU);
  //PCSetType(pc,PCSOR);
  
  PCGetType(pc,&pctype);
  PetscInt mmax;
  
  KSPGetTolerances(ksp,NULL,NULL,NULL,&mmax);
  
  PetscPrintf(PETSC_COMM_WORLD,"################# Linear solver settings #####################\n");
  PetscPrintf(PETSC_COMM_WORLD,"# Main solver: %s, prec.: %s, maxiter.: %i \n",ksptype,pctype,mmax);
  
  
  KSPSolve(ksp,F,X);
  
  PetscInt niter;
  PetscScalar rnorm;
  KSPGetIterationNumber(ksp,&niter);
  KSPGetResidualNorm(ksp,&rnorm);
  PetscPrintf(PETSC_COMM_WORLD,"State solver:  iter: %i, rerr.: %e, \n",niter,rnorm);
  
  // Check solution 1
  PetscScalar normSol;
  VecNorm(X,NORM_2,&normSol);
  printf("Norm of Solution: %e\n",normSol);
  
  // Check solution 2
  ComputeSomething(dm, topMatrix, X);
  
  {
    PetscViewer viewer;
    const char* fn = "Test_X.vtk";
    PetscViewerVTKOpen(PETSC_COMM_WORLD,fn,FILE_MODE_WRITE,&viewer);
    VecView(X,viewer);
    PetscViewerDestroy(&viewer);
  }
  
  {
    PetscViewer viewer;
    const char* fn = "Test_F.vtk";
    PetscViewerVTKOpen(PETSC_COMM_WORLD,fn,FILE_MODE_WRITE,&viewer);
    VecView(F,viewer);
    PetscViewerDestroy(&viewer);
  }
  
  {
    PetscViewer viewer;
    const char* fn = "Test_N.vtk";
    PetscViewerVTKOpen(PETSC_COMM_WORLD,fn,FILE_MODE_WRITE,&viewer);
    VecView(N,viewer);
    PetscViewerDestroy(&viewer);
  }
}

int main(int argc, char **argv) {
  PetscErrorCode ierr;
  static char help[] = "";
  ierr = PetscInitialize(&argc, &argv, NULL, help);
  CHKERRQ(ierr);
  SolveFEM();
  PetscSynchronizedFlush(PETSC_COMM_WORLD,PETSC_STDOUT);
  PetscFinalize();
  
  return 0;
}


PetscInt Hex8Isoparametric(PetscScalar *X, PetscScalar *Y, PetscScalar *Z, PetscScalar nu, PetscInt redInt, PetscScalar *ke){
  // HEX8_ISOPARAMETRIC - Computes HEX8 isoparametric element matrices
  // The element stiffness matrix is computed as:
  //
  //       ke = int(int(int(B^T*C*B,x),y),z)
  //
  // For an isoparameteric element this integral becomes:
  //
  //       ke = int(int(int(B^T*C*B*det(J),xi=-1..1),eta=-1..1),zeta=-1..1)
  //
  // where B is the more complicated expression:
  // B = [dx*alpha1 + dy*alpha2 + dz*alpha3]*N
  // where
  // dx = [invJ11 invJ12 invJ13]*[dxi deta dzeta]
  // dy = [invJ21 invJ22 invJ23]*[dxi deta dzeta]
  // dy = [invJ31 invJ32 invJ33]*[dxi deta dzeta]
  //
  // Remark: The elasticity modulus is left out in the below
  // computations, because we multiply with it afterwards (the aim is
  // topology optimization).
  // Furthermore, this is not the most efficient code, but it is readable.
  //
  /////////////////////////////////////////////////////////////////////////////////
  //////// INPUT:
  // X, Y, Z  = Vectors containing the coordinates of the eight nodes
  //               (x1,y1,z1,x2,y2,z2,...,x8,y8,z8). Where node 1 is in the lower
  //               left corner, and node 2 is the next node counterclockwise
  //               (looking in the negative z-dir).
  //               Finish the x-y-plane and then move in the positive z-dir.
  // redInt   = Reduced integration option boolean (here an integer).
  //           	redInt == 0 (false): Full integration
  //           	redInt == 1 (true): Reduced integration
  // nu 		= Poisson's ratio.
  //
  //////// OUTPUT:
  // ke  = Element stiffness matrix. Needs to be multiplied with elastic modulus
  //
  //   Written 2013 at
  //   Department of Mechanical Engineering
  //   Technical University of Denmark (DTU).
  /////////////////////////////////////////////////////////////////////////////////
  // Numbering:
  //%    8----7
  //%   /|   /|
  //%  / |  / |
  //% 5----6  |
  //% |  4-|--3
  //% | /  | / 
  //% |/   |/
  //% 1----2   (- x, / y, | z)
  
  //// COMPUTE ELEMENT STIFFNESS MATRIX
  // Lame's parameters (with E=1.0):
  PetscScalar lambda = nu/((1.0+nu)*(1.0-2.0*nu));
  PetscScalar mu = 1.0/(2.0*(1.0+nu));
  // Constitutive matrix
  PetscScalar C[6][6] = {{lambda+2.0*mu, lambda, lambda, 0.0, 0.0, 0.0},
  {lambda, lambda+2.0*mu, lambda, 0.0, 0.0, 0.0},
  {lambda, lambda, lambda+2.0*mu, 0.0, 0.0, 0.0},
  {0.0,    0.0,    0.0,           mu,  0.0, 0.0},
  {0.0, 	0.0, 	0.0, 		   0.0, mu,  0.0}, 
  {0.0, 	0.0,	0.0, 		   0.0, 0.0, mu}};
  // Gauss points (GP) and weigths
  // Two Gauss points in all directions (total of eight)
  PetscScalar GP[2] = {-0.577350269189626, 0.577350269189626}; 
  // Corresponding weights
  PetscScalar W[2] = {1.0, 1.0};
  // If reduced integration only use one GP
  if (redInt){
    GP[1] = 0.0;
    W[1] = 2.0;
  }
  // Matrices that help when we gather the strain-displacement matrix:
  PetscScalar alpha1[6][3]; PetscScalar alpha2[6][3]; PetscScalar alpha3[6][3];
  memset(alpha1, 0, sizeof(alpha1[0][0])*6*3); // zero out
  memset(alpha2, 0, sizeof(alpha2[0][0])*6*3); // zero out
  memset(alpha3, 0, sizeof(alpha3[0][0])*6*3); // zero out
  alpha1[0][0] = 1.0; alpha1[3][1] = 1.0; alpha1[5][2] = 1.0;
  alpha2[1][1] = 1.0; alpha2[3][0] = 1.0; alpha2[4][2] = 1.0;
  alpha3[2][2] = 1.0; alpha3[4][1] = 1.0; alpha3[5][0] = 1.0;
  PetscScalar dNdxi[8]; PetscScalar dNdeta[8]; PetscScalar dNdzeta[8];
  PetscScalar J[3][3];
  PetscScalar invJ[3][3];
  PetscScalar beta[6][3];
  PetscScalar B[6][24]; // Note: Small enough to be allocated on stack
  PetscScalar *dN;
  // Make sure the stiffness matrix is zeroed out:
  memset(ke, 0, sizeof(ke[0])*24*24);
  // Perform the numerical integration
  for (PetscInt ii=0; ii<2-redInt; ii++){
    for (PetscInt jj=0; jj<2-redInt; jj++){
      for (PetscInt kk=0; kk<2-redInt; kk++){
	// Integration point
	PetscScalar xi = GP[ii]; 
	PetscScalar eta = GP[jj]; 
	PetscScalar zeta = GP[kk];
	// Differentiated shape functions
	DifferentiatedShapeFunctions(xi, eta, zeta, dNdxi, dNdeta, dNdzeta);
	// Jacobian
	J[0][0] = Dot(dNdxi,X,8); J[0][1] = Dot(dNdxi,Y,8); J[0][2] = Dot(dNdxi,Z,8);
	J[1][0] = Dot(dNdeta,X,8); J[1][1] = Dot(dNdeta,Y,8); J[1][2] = Dot(dNdeta,Z,8);
	J[2][0] = Dot(dNdzeta,X,8); J[2][1] = Dot(dNdzeta,Y,8); J[2][2] = Dot(dNdzeta,Z,8);
	// Inverse and determinant
	PetscScalar detJ = Inverse3M(J, invJ);
	if (detJ<0){
	  //		printf("Negative Jacobian\n");
	}
	// Weight factor at this point
	PetscScalar weight = W[ii]*W[jj]*W[kk]*detJ;
	// Strain-displacement matrix
	memset(B, 0, sizeof(B[0][0])*6*24); // zero out
	for (PetscInt ll=0; ll<3; ll++){
	  // Add contributions from the different derivatives
	  if (ll==0) {dN = dNdxi;}
	  if (ll==1) {dN = dNdeta;}
	  if (ll==2) {dN = dNdzeta;}
	  // Assemble strain operator
	  for (PetscInt i=0; i<6; i++){
	    for (PetscInt j=0; j<3; j++){
	      beta[i][j] = invJ[0][ll]*alpha1[i][j]
	      +invJ[1][ll]*alpha2[i][j]
	      +invJ[2][ll]*alpha3[i][j];
	    }
	  }
	  // Add contributions to strain-displacement matrix
	  for (PetscInt i=0; i<6; i++){
	    for (PetscInt j=0; j<24; j++){
	      B[i][j] = B[i][j] + beta[i][j%3]*dN[j/3];
	    }
	  }
	}
	// Finally, add to the element matrix
	for (PetscInt i=0; i<24; i++){
	  for (PetscInt j=0; j<24; j++){
	    for (PetscInt k=0; k<6; k++){
	      for (PetscInt l=0; l<6; l++){
		
		ke[j+24*i] = ke[j+24*i] + weight*(B[k][i] * C[k][l] * B[l][j]);
	      }
	    }
	  }
	}
      }
    }
  }
  return 0;
}
PetscScalar Dot(PetscScalar *v1, PetscScalar *v2, PetscInt l){
  // Function that returns the dot product of v1 and v2,
  // which must have the same length l
  PetscScalar result = 0.0;
  for (PetscInt i=0; i<l; i++){
    result = result + v1[i]*v2[i];
  }
  return result;
}

void DifferentiatedShapeFunctions(PetscScalar xi, PetscScalar eta, PetscScalar zeta, PetscScalar *dNdxi, PetscScalar *dNdeta, PetscScalar *dNdzeta){
  //differentiatedShapeFunctions - Computes differentiated shape functions
  // At the point given by (xi, eta, zeta).
  // With respect to xi:
  dNdxi[0]  = -0.125*(1.0-eta)*(1.0-zeta);
  dNdxi[1]  =  0.125*(1.0-eta)*(1.0-zeta);
  dNdxi[2]  =  0.125*(1.0+eta)*(1.0-zeta);
  dNdxi[3]  = -0.125*(1.0+eta)*(1.0-zeta);
  dNdxi[4]  = -0.125*(1.0-eta)*(1.0+zeta);
  dNdxi[5]  =  0.125*(1.0-eta)*(1.0+zeta);
  dNdxi[6]  =  0.125*(1.0+eta)*(1.0+zeta);
  dNdxi[7]  = -0.125*(1.0+eta)*(1.0+zeta);
  // With respect to eta:
  dNdeta[0] = -0.125*(1.0-xi)*(1.0-zeta);
  dNdeta[1] = -0.125*(1.0+xi)*(1.0-zeta);
  dNdeta[2] =  0.125*(1.0+xi)*(1.0-zeta);
  dNdeta[3] =  0.125*(1.0-xi)*(1.0-zeta);
  dNdeta[4] = -0.125*(1.0-xi)*(1.0+zeta);
  dNdeta[5] = -0.125*(1.0+xi)*(1.0+zeta);
  dNdeta[6] =  0.125*(1.0+xi)*(1.0+zeta);
  dNdeta[7] =  0.125*(1.0-xi)*(1.0+zeta);
  // With respect to zeta:
  dNdzeta[0]= -0.125*(1.0-xi)*(1.0-eta);
  dNdzeta[1]= -0.125*(1.0+xi)*(1.0-eta);
  dNdzeta[2]= -0.125*(1.0+xi)*(1.0+eta);
  dNdzeta[3]= -0.125*(1.0-xi)*(1.0+eta);
  dNdzeta[4]=  0.125*(1.0-xi)*(1.0-eta);
  dNdzeta[5]=  0.125*(1.0+xi)*(1.0-eta);
  dNdzeta[6]=  0.125*(1.0+xi)*(1.0+eta);
  dNdzeta[7]=  0.125*(1.0-xi)*(1.0+eta);
}

PetscScalar Inverse3M(PetscScalar J[][3], PetscScalar invJ[][3]){
  //inverse3M - Computes the inverse of a 3x3 matrix
  PetscScalar detJ = J[0][0]*(J[1][1]*J[2][2]-J[2][1]*J[1][2])-J[0][1]*(J[1][0]*J[2][2]-J[2][0]*J[1][2])+J[0][2]*(J[1][0]*J[2][1]-J[2][0]*J[1][1]);
  invJ[0][0] = (J[1][1]*J[2][2]-J[2][1]*J[1][2])/detJ;
  invJ[0][1] = -(J[0][1]*J[2][2]-J[0][2]*J[2][1])/detJ;
  invJ[0][2] = (J[0][1]*J[1][2]-J[0][2]*J[1][1])/detJ;
  invJ[1][0] = -(J[1][0]*J[2][2]-J[1][2]*J[2][0])/detJ;
  invJ[1][1] = (J[0][0]*J[2][2]-J[0][2]*J[2][0])/detJ;
  invJ[1][2] = -(J[0][0]*J[1][2]-J[0][2]*J[1][0])/detJ;
  invJ[2][0] = (J[1][0]*J[2][1]-J[1][1]*J[2][0])/detJ;
  invJ[2][1] = -(J[0][0]*J[2][1]-J[0][1]*J[2][0])/detJ;
  invJ[2][2] = (J[0][0]*J[1][1]-J[1][0]*J[0][1])/detJ;
  return detJ;
}