diff --git a/src/.pmpo_MPMesh_assembly.hpp.swp b/src/.pmpo_MPMesh_assembly.hpp.swp
new file mode 100644
index 0000000..4eb414f
Binary files /dev/null and b/src/.pmpo_MPMesh_assembly.hpp.swp differ
diff --git a/src/pmpo_MPMesh.cpp b/src/pmpo_MPMesh.cpp
index 3d3992a..022c9f8 100644
--- a/src/pmpo_MPMesh.cpp
+++ b/src/pmpo_MPMesh.cpp
@@ -293,6 +293,8 @@ void MPMesh::T2LTracking(Vec2dView dx){
 }
 
 void MPMesh::reconstructSlices() {
+    
+    std::cout<<__FUNCTION__<<std::endl;
     if (reconstructSlice.size() == 0) return;
     Kokkos::Timer timer;
     calcBasis();
@@ -304,6 +306,7 @@ void MPMesh::reconstructSlices() {
 }
 
 void MPMesh::push(){
+  std::cout<<__FUNCTION__<<std::endl;
   Kokkos::Timer timer;
   p_mesh->computeRotLatLonIncr();
   sphericalInterpolation<MeshF_RotLatLonIncr>(*this);
diff --git a/src/pmpo_MPMesh_assembly.hpp b/src/pmpo_MPMesh_assembly.hpp
index 2f81aa6..984d08a 100644
--- a/src/pmpo_MPMesh_assembly.hpp
+++ b/src/pmpo_MPMesh_assembly.hpp
@@ -69,10 +69,14 @@ void MPMesh::assemblyVtx0(){
 
 template <MeshFieldIndex meshFieldIndex>
 void MPMesh::assemblyElm0() {
+  
+  std::cout<<__FUNCTION__<<std::endl;	
   Kokkos::Timer timer;
   constexpr MaterialPointSlice mpfIndex = meshFieldIndexToMPSlice<meshFieldIndex>;
   auto mpData = p_MPs->getData<mpfIndex>();
   const int numEntries = mpSliceToNumEntries<mpfIndex>();
+  auto mpPositions = p_MPs->getData<MPF_Cur_Pos_XYZ>();
+  auto mpAppID = p_MPs->getData<polyMPO::MPF_MP_APP_ID>();
 
   int numElms = p_mesh->getNumElements();
   p_mesh->fillMeshField<meshFieldIndex>(numElms, numEntries, 0.0);
@@ -100,29 +104,106 @@ void MPMesh::assemblyElm0() {
 
 template <MeshFieldIndex meshFieldIndex>
 void MPMesh::assemblyVtx1() {
+
+  std::cout<<__FUNCTION__<<std::endl;
+  static int count=0;
   constexpr MaterialPointSlice mpfIndex = meshFieldIndexToMPSlice<meshFieldIndex>;
   auto elm2VtxConn = p_mesh->getElm2VtxConn();  
-  auto mpData = p_MPs->getData<mpfIndex>();
   const int numEntries = mpSliceToNumEntries<mpfIndex>();
   int numVtx = p_mesh->getNumVertices();
 
+  auto mpData = p_MPs->getData<mpfIndex>();
+  auto weight = p_MPs->getData<MPF_Basis_Vals>();
+  auto mpPositions = p_MPs->getData<MPF_Cur_Pos_XYZ>();
+  auto mpAppID = p_MPs->getData<polyMPO::MPF_MP_APP_ID>();
+
   p_mesh->fillMeshField<meshFieldIndex>(numVtx, numEntries, 0.0);
   auto meshField = p_mesh->getMeshField<meshFieldIndex>();
 
+  Kokkos::View<double*[4][4]> VtxMatrices("VtxMatrices", p_mesh->getNumVertices());
+  auto vtxCoords = p_mesh->getMeshField<polyMPO::MeshF_VtxCoords>();
+
+  double radius = p_mesh->getSphereRadius();
+
+  //Assemble matrix for each vertex
   auto assemble = PS_LAMBDA(const int& elm, const int& mp, const int& mask) {
     if(mask) { //if material point is 'active'/'enabled'
       int nVtxE = elm2VtxConn(elm,0); //number of vertices bounding the element
       for(int i=0; i<nVtxE; i++){
         int vID = elm2VtxConn(elm,i+1)-1; //vID = vertex id
-        double fieldComponentVal;
-        for(int j=0;j<numEntries;j++){
-          fieldComponentVal = mpData(mp,j);
-          Kokkos::atomic_add(&meshField(vID,j),fieldComponentVal);
-        }
+        double w_vtx=weight(mp,i);
+        
+        double CoordDiffs[4] = {1, (vtxCoords(vID,0) - mpPositions(mp,0))/radius, (vtxCoords(vID,1) - mpPositions(mp,1))/radius, 
+          (vtxCoords(vID,2) - mpPositions(mp,2))/radius};		            //add to the matrix
+        for (int k=0; k<4; k++)
+          for(int l=0;l<4;l++)
+            Kokkos::atomic_add(&VtxMatrices(vID,k,l), CoordDiffs[k] * CoordDiffs[l] * w_vtx);
       }
     }
   };
   p_MPs->parallel_for(assemble, "assembly");
+  
+  //Solve Ax=b for each vertex  
+  Kokkos::View<double*[4]> VtxCoeffs("VtxMatrices", p_mesh->getNumVertices());
+  Kokkos::parallel_for("solving Ax=b", numVtx, KOKKOS_LAMBDA(const int vtx){
+    Vec4d v0 = {VtxMatrices(vtx,0,0), VtxMatrices(vtx,0,1), VtxMatrices(vtx,0,2), VtxMatrices(vtx,0,3)};
+    Vec4d v1 = {VtxMatrices(vtx,1,0), VtxMatrices(vtx,1,1), VtxMatrices(vtx,1,2), VtxMatrices(vtx,1,3)};
+    Vec4d v2 = {VtxMatrices(vtx,2,0), VtxMatrices(vtx,2,1), VtxMatrices(vtx,2,2), VtxMatrices(vtx,2,3)};
+    Vec4d v3 = {VtxMatrices(vtx,3,0), VtxMatrices(vtx,3,1), VtxMatrices(vtx,3,2), VtxMatrices(vtx,3,3)};
+    Matrix A = {v0,v1,v2,v3};
+    double coeff[4]={0.0, 0.0, 0.0, 0.0};
+    CholeskySolve(A, coeff);
+    for (int i=0; i<4; i++) VtxCoeffs(vtx,i)=coeff[i];
+    
+    if(vtx >= 20 && vtx<=24){
+      printf("Vtx %d coeff %.15e %.15e %.15e %.15e \n", vtx, coeff[0], coeff[1], coeff[2], coeff[3]);
+    }  
+  });
+ 
+  //Reconstruct
+  Kokkos::View<double*> reconVals("meshField", p_mesh->getNumVertices());
+  auto reconstruct = PS_LAMBDA(const int& elm, const int& mp, const int& mask) {
+    if(mask) { //if material point is 'active'/'enabled'
+      int nVtxE = elm2VtxConn(elm,0); //number of vertices bounding the element
+      for(int i=0; i<nVtxE; i++){
+        int vID = elm2VtxConn(elm,i+1)-1;
+        double w_vtx=weight(mp,i); 
+        double CoordDiffs[4] = {1, (vtxCoords(vID,0) - mpPositions(mp,0))/radius, (vtxCoords(vID,1) - mpPositions(mp,1))/radius, 
+          (vtxCoords(vID,2) - mpPositions(mp,2))/radius};
+	auto val = w_vtx*(VtxCoeffs(vID,0) + VtxCoeffs(vID,1)*CoordDiffs[1] + VtxCoeffs(vID,2)*CoordDiffs[2] + VtxCoeffs(vID,3)*CoordDiffs[3])*mpData(mp,0) ;
+	Kokkos::atomic_add(&reconVals(vID), val);
+        if(vID>=20 && vID<=24){
+           printf("Vtx %d coeff val %.15e value %.15e\n", vID, val, reconVals(vID));
+        }
+      }
+    }
+  };
+  p_MPs->parallel_for(reconstruct, "reconstruct");
+  
+  Kokkos::parallel_for("averaging", numVtx, KOKKOS_LAMBDA(const int vtx){
+    meshField(vtx, 0) = reconVals(vtx);
+  });
+  
+  count++; 
+  //if(count>1) exit(1);
+  //Debugging
+  bool debug=false;
+  if(!debug) return;
+  int numElms = p_mesh->getNumElements();
+  Kokkos::parallel_for("counting", numElms, KOKKOS_LAMBDA(const int elm){
+    int nVtxE = elm2VtxConn(elm,0);
+    if(elm==4 || elm==5){
+      for(int i=0; i<nVtxE; i++){
+        int vID = elm2VtxConn(elm,i+1)-1; //vID = vertex id
+        printf("Vertex id %d \n", vID);
+	printf("Matrix %.15e %.15e %.15e %.15e \n", VtxMatrices(vID,0,0),VtxMatrices(vID,0,1),VtxMatrices(vID,0,2),VtxMatrices(vID,0,3));
+        printf("Matrix %.15e %.15e %.15e \n", VtxMatrices(vID,1,1),VtxMatrices(vID,1,2),VtxMatrices(vID,1,3));
+        printf("Matrix %.15e %.15e \n", VtxMatrices(vID,2,2),VtxMatrices(vID,2,3));
+        printf("Matrix %.15e \n", VtxMatrices(vID,3,3));
+      }
+    }
+  });
+  
 }
 
 template <MeshFieldIndex meshFieldIndex>
diff --git a/src/pmpo_c.cpp b/src/pmpo_c.cpp
index b37baa7..fef4b94 100644
--- a/src/pmpo_c.cpp
+++ b/src/pmpo_c.cpp
@@ -255,6 +255,8 @@ void polympo_setMPPositions_f(MPMesh_ptr p_mpmesh,
       mpPositions(mp,0) = mpPositionsIn_d(0, mpAppID(mp));
       mpPositions(mp,1) = mpPositionsIn_d(1, mpAppID(mp));
       mpPositions(mp,2) = mpPositionsIn_d(2, mpAppID(mp));
+      //printf("Mp %d Map mpAppID(mp) %d Pos %.15e %.15e %.15e \n", mp, mpAppID(mp), 
+        //mpPositions(mp,0), mpPositions(mp,1), mpPositions(mp,2));
     }
   };
   p_MPs->parallel_for(setPos, "setMPPositions");
@@ -926,6 +928,8 @@ void polympo_push_f(MPMesh_ptr p_mpmesh){
 
 //TODO skeleton of reconstruction functions
 void polympo_setReconstructionOfMass_f(MPMesh_ptr p_mpmesh, const int order, const int meshEntType){
+ 
+  std::cout<<__FUNCTION__<<std::endl;	
   checkMPMeshValid(p_mpmesh);
   auto mpmesh = ((polyMPO::MPMesh*)p_mpmesh);
   polyMPO::MeshFieldType type = static_cast<polyMPO::MeshFieldType>(meshEntType);
diff --git a/src/pmpo_utils.hpp b/src/pmpo_utils.hpp
index af6e397..c3fd754 100644
--- a/src/pmpo_utils.hpp
+++ b/src/pmpo_utils.hpp
@@ -21,14 +21,22 @@ void PMT_Assert_Fail(const char* msg) __attribute__ ((noreturn));
 
 namespace polyMPO{
 
+const double EPSILON = std::numeric_limits<double>::epsilon();
+
 #define vec2d_nEntries 2
 typedef double vec2d_t[vec2d_nEntries];
 #define vec3d_nEntries 3
 typedef double vec3d_t[vec3d_nEntries];
+#define vec4d_nEntries 4
+typedef double vec4d_t[vec4d_nEntries];
+
 typedef double doubleSclr_t[1];
 
 class Vec2d;
 class Vec3d;
+class Vec4d;
+class Matrix;
+
 using Vec2dView = Kokkos::View<Vec2d*>;
 using Vec3dView = Kokkos::View<Vec3d*>;
 using DoubleView = Kokkos::View<double*>;
@@ -145,6 +153,103 @@ class Vec3d{
     }
 };
 
+
+class Vec4d{
+    private:
+    vec4d_t coords_;
+    public:
+      
+      //constructors
+      KOKKOS_INLINE_FUNCTION
+      Vec4d():coords_{0.0, 0.0, 0.0, 0.0}{}
+      ~Vec4d() = default;
+               
+      KOKKOS_INLINE_FUNCTION
+      Vec4d(double x1, double x2, double x3, double x4):coords_{x1,x2,x3,x4}{}
+                          
+      KOKKOS_INLINE_FUNCTION
+      Vec4d(double x[4]):coords_{x[0], x[1], x[2], x[3]}{}
+
+      //operators
+      KOKKOS_INLINE_FUNCTION
+      double operator[](int i) const { return coords_[i]; }
+                                           
+      KOKKOS_INLINE_FUNCTION
+      double &operator[](int i) { return coords_[i]; }
+                                                                
+      KOKKOS_INLINE_FUNCTION
+      Vec4d operator+(const Vec4d& v) const {
+          return Vec4d(coords_[0] + v.coords_[0], coords_[1] + v.coords_[1], 
+              coords_[2] + v.coords_[2], coords_[3] + v.coords_[3]);
+      }
+      
+      KOKKOS_INLINE_FUNCTION
+      Vec4d operator-(const Vec4d& v) const {
+          return Vec4d(coords_[0] - v.coords_[0], coords_[1] - v.coords_[1], 
+              coords_[2] - v.coords_[2], coords_[3] - v.coords_[3]);
+      }                                                                                                                                              
+      
+      KOKKOS_INLINE_FUNCTION
+      Vec4d operator-() { return Vec4d(-coords_[0], -coords_[1], -coords_[2], -coords_[3]); }
+      
+      KOKKOS_INLINE_FUNCTION
+      Vec4d operator*(double scalar) const {
+          return Vec4d(coords_[0] * scalar, coords_[1] * scalar, coords_[2] * scalar, coords_[3] * scalar);
+      }
+      
+      KOKKOS_INLINE_FUNCTION
+      double dot(const Vec4d& v) const {
+          return coords_[0] * v.coords_[0] + coords_[1] * v.coords_[1] + coords_[2] * v.coords_[2] + coords_[3] * v.coords_[3];
+      }
+      
+      KOKKOS_INLINE_FUNCTION
+      double magnitude() const {
+         return std::sqrt(coords_[0] * coords_[0] + coords_[1] * coords_[1] + coords_[2] * coords_[2] + coords_[3] * coords_[3]);
+      }
+};
+
+class Matrix {
+    
+    private:
+  
+        double** data_;
+        int rows_;
+        int cols_;
+   
+    public:
+			        
+        KOKKOS_INLINE_FUNCTION
+        Matrix(Vec4d v0, Vec4d v1, Vec4d v2, Vec4d v3){
+            rows_ = 4;
+            cols_ = 4;
+            data_ = new double*[rows_];
+	    
+	    for (int i=0; i<rows_; i++){
+	        data_[i] = new double[cols_];
+            }
+
+            for (int i=0; i<cols_; i++){
+	        data_[0][i] = v0[i];
+                data_[1][i] = v1[i];
+		data_[2][i] = v2[i];
+		data_[3][i] = v3[i];
+	    }
+	}
+	
+	KOKKOS_INLINE_FUNCTION
+	~Matrix(){
+	    for (int i=0; i<4; i++){
+	        delete[] data_[i];
+            }
+	    delete[] data_;
+	}
+        
+	KOKKOS_INLINE_FUNCTION
+	double& operator()(int i, int j) { return data_[i][j];}
+				        
+};
+
+
 KOKKOS_INLINE_FUNCTION
 void initArray(Vec2d* arr, int n, Vec2d fill){
     for(int i=0; i<n; i++){
@@ -152,6 +257,73 @@ void initArray(Vec2d* arr, int n, Vec2d fill){
     }
 }
 
+
+KOKKOS_INLINE_FUNCTION
+void CholeskySolve(Matrix& A, double* x){
+
+    double a_00=A(0,0);
+    if (A(0,0)==0){
+        x[0]=0;
+	x[1]=0;
+	x[2]=0;
+	x[3]=0;
+	return;
+    }
+    
+    A(0,0) = std::sqrt(A(0,0));
+    A(0,1) /= A(0,0);
+    A(0,2) /= A(0,0);
+    A(0,3) /= A(0,0);
+
+    double diag = A(1,1) - A(0,1)*A(0,1);
+    if(diag>EPSILON){
+         A(1,1)=std::sqrt(diag);
+	 A(1,2)=(A(1,2)-A(0,1)*A(0,2))/A(1,1);
+	 A(1,3)=(A(1,3)-A(0,1)*A(0,3))/A(1,1);
+    }
+    else{
+        A(1,1)=0.0;
+	A(1,2)=0.0;
+	A(1,3)=0.0;
+    }
+    
+    diag = A(2,2) - A(0,2)*A(0,2)-A(1,2)*A(1,2);
+    if(diag>EPSILON){
+        A(2,2)=std::sqrt(diag);
+        A(2,3)=(A(2,3)-A(0,2)*A(0,3)-A(1,2)*A(1,3))/A(2,2);
+    }
+    else{
+       A(2,2)=0.0;
+       A(2,3)=0.0;
+    }
+
+    diag=A(3,3)-A(0,3)*A(0,3)-A(1,3)*A(1,3)-A(2,3)*A(2,3);
+    if(diag>EPSILON)
+       A(3,3)=std::sqrt(diag);
+    else 
+       A(3,3)=0.0;
+
+    if(A(1,1)<EPSILON || A(2,2)<EPSILON || A(3,3)<EPSILON){
+       x[0]=1.0/a_00;
+       x[1]=0.0;
+       x[2]=0.0;
+       x[3]=0.0;
+    }
+    else{
+       x[0]= 1.0/A(0,0);
+       x[1]= -A(0,1)*x[0]/A(1,1);                //- m12 % array(iVertex)*a0(iVertex)/m22 % array(iVertex)
+       x[2]= -(A(0,2)*x[0]+A(1,2)*x[1])/A(2,2);  //-(m13 % array(iVertex)*a0(iVertex) + m23 % array(iVertex)*a1(iVertex))/m33 % array(iVertex)
+       x[3]= -(A(0,3)*x[0]+A(1,3)*x[1]+A(2,3)*x[2])/A(3,3); //-(m14 % array(iVertex)*a0(iVertex) + m24 % array(iVertex)*a1(iVertex) + m34 % array(iVertex)*a2(iVertex))/m44 % array(iVertex)
+
+       x[3] = x[3]/A(3,3);
+       x[2] = ( x[2] - A(2,3)*x[3] )/A(2,2);
+       x[1] = ( x[1] - A(1,2)*x[2] - A(1,3)*x[3])/A(1,1);
+       x[0] = ( x[0] - A(0,1)*x[1] - A(0,2)*x[2] - A(0,3)*x[3])/A(0,0);
+
+    } 
+}
+
+
 KOKKOS_INLINE_FUNCTION
 void initArray(Vec3d* arr, int n, Vec3d fill){
     for(int i=0; i<n; i++){