diff --git a/src/pmpo_MPMesh_assembly.hpp b/src/pmpo_MPMesh_assembly.hpp
index 2f81aa6..733fd16 100644
--- a/src/pmpo_MPMesh_assembly.hpp
+++ b/src/pmpo_MPMesh_assembly.hpp
@@ -83,7 +83,7 @@ void MPMesh::assemblyElm0() {
     if(mask) { //if material point is 'active'/'enabled'
       Kokkos::atomic_add(&mpsPerElm(elm),1);
       for(int j=0;j<numEntries;j++){
-        Kokkos::atomic_add(&meshField(elm,j), mpData(mp,0));
+        Kokkos::atomic_add(&meshField(elm,j), mpData(mp,j));
       }
     }
   };
@@ -100,29 +100,100 @@ void MPMesh::assemblyElm0() {
 
 template <MeshFieldIndex meshFieldIndex>
 void MPMesh::assemblyVtx1() {
-  constexpr MaterialPointSlice mpfIndex = meshFieldIndexToMPSlice<meshFieldIndex>;
+  //Mesh Information
   auto elm2VtxConn = p_mesh->getElm2VtxConn();  
-  auto mpData = p_MPs->getData<mpfIndex>();
-  const int numEntries = mpSliceToNumEntries<mpfIndex>();
   int numVtx = p_mesh->getNumVertices();
+  auto vtxCoords = p_mesh->getMeshField<polyMPO::MeshF_VtxCoords>();
 
+  //Mesh Field
+  constexpr MaterialPointSlice mpfIndex = meshFieldIndexToMPSlice<meshFieldIndex>;
+  const int numEntries = mpSliceToNumEntries<mpfIndex>();
   p_mesh->fillMeshField<meshFieldIndex>(numVtx, numEntries, 0.0);
   auto meshField = p_mesh->getMeshField<meshFieldIndex>();
 
+  //Material Points
+  auto mpData = p_MPs->getData<mpfIndex>();
+  auto weight = p_MPs->getData<MPF_Basis_Vals>();
+  auto mpPositions = p_MPs->getData<MPF_Cur_Pos_XYZ>();
+
+  //Matrix for each vertex
+  Kokkos::View<double*[vec4d_nEntries][vec4d_nEntries]> VtxMatrices("VtxMatrices", p_mesh->getNumVertices());
+
+  //Reconstructed values
+  Kokkos::View<double**> reconVals("meshField", p_mesh->getNumVertices(), numEntries);
+  
+  //Earth Radius
+  double radius = 1.0;
+  if(p_mesh->getGeomType() == geom_spherical_surf)
+    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[vec4d_nEntries] = {1, (vtxCoords(vID,0) - mpPositions(mp,0))/radius, 
+                                                (vtxCoords(vID,1) - mpPositions(mp,1))/radius, 
+                                                (vtxCoords(vID,2) - mpPositions(mp,2))/radius};		 
+        for (int k=0; k<vec4d_nEntries; k++)
+          for (int l=0; l<vec4d_nEntries; 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*[vec4d_nEntries]> 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)};
+    Matrix4d A = {v0,v1,v2,v3};
+
+    //double f_norm=A.frobeniusNorm();
+    double A_trace = A.trace();
+    Matrix4d A_regularized = {v0, v1, v2, v3};
+    A_regularized.addToDiag(A_trace*1e-8);
+ 
+    double coeff[vec4d_nEntries]={0.0, 0.0, 0.0, 0.0};
+    CholeskySolve4d_UnitRHS(A_regularized, coeff);
+    for (int i=0; i<vec4d_nEntries; i++) 
+      VtxCoeffs(vtx,i)=coeff[i];
+  });
+ 
+  //Reconstruct
+  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[vec4d_nEntries] = {1, (vtxCoords(vID,0) - mpPositions(mp,0))/radius,
+                                                (vtxCoords(vID,1) - mpPositions(mp,1))/radius, 
+                                                (vtxCoords(vID,2) - mpPositions(mp,2))/radius};
+        
+        auto factor = w_vtx*(VtxCoeffs(vID,0) + VtxCoeffs(vID,1)*CoordDiffs[1] + 
+                                                VtxCoeffs(vID,2)*CoordDiffs[2] + 
+                                                VtxCoeffs(vID,3)*CoordDiffs[3]);
+  
+        for (int k=0; k<numEntries; k++){
+          auto val = factor*mpData(mp,k);
+          Kokkos::atomic_add(&reconVals(vID,k), val);
+        }
+      }
+    }
+  };
+  p_MPs->parallel_for(reconstruct, "reconstruct");
+  
+  Kokkos::parallel_for("assigning", numVtx, KOKKOS_LAMBDA(const int vtx){
+    for(int k=0; k<numEntries; k++)
+      meshField(vtx, k) = reconVals(vtx,k);
+  });
 }
 
 template <MeshFieldIndex meshFieldIndex>
diff --git a/src/pmpo_materialPoints.hpp b/src/pmpo_materialPoints.hpp
index 4c37a16..77a325c 100644
--- a/src/pmpo_materialPoints.hpp
+++ b/src/pmpo_materialPoints.hpp
@@ -211,25 +211,26 @@ class MaterialPoints {
         auto tgtPosXYZ = MPs->get<MPF_Tgt_Pos_XYZ>();
         auto rotLatLonIncr = MPs->get<MPF_Rot_Lat_Lon_Incr>();
         
+        auto is_rotated = getRotatedFlag();
         auto updateRotLatLon = PS_LAMBDA(const int& elm, const int& mp, const int& mask){
             if(mask){
                 auto rotLat = curPosRotLatLon(mp,0) + rotLatLonIncr(mp,0); // phi
-                auto rotLon = curPosRotLatLon(mp,1) + rotLatLonIncr(mp,1); // lambda
+                auto rotLon = curPosRotLatLon(mp,1) + rotLatLonIncr(mp,1); // lambda   
+                tgtPosRotLatLon(mp,0) = rotLat;
+                tgtPosRotLatLon(mp,1) = rotLon;        
                 auto geoLat = rotLat;
                 auto geoLon = rotLon;
-                tgtPosRotLatLon(mp,0) = geoLat;
-                tgtPosRotLatLon(mp,1) = geoLon;
+                if(is_rotated){
+                  auto xyz_rot = xyz_from_lat_lon(rotLat, rotLon, radius);
+                  auto xyz_geo = grid_rotation_backward(xyz_rot);
+                  lat_lon_from_xyz(geoLat, geoLon, xyz_geo, radius);
+                }	
                 // x = cosLon cosLat, y = sinLon cosLat, z = sinLat
                 tgtPosXYZ(mp,0) = radius * std::cos(geoLon) * std::cos(geoLat);
                 tgtPosXYZ(mp,1) = radius * std::sin(geoLon) * std::cos(geoLat);
-                tgtPosXYZ(mp,2) = radius * std::sin(geoLat); 
+                tgtPosXYZ(mp,2) = radius * std::sin(geoLat);
             } 
         };
-        if(isRotatedFlag){
-            //TODO rotation lat lon calc
-            fprintf(stderr, "rotational lat lon in MP is not support yet!");
-            PMT_ALWAYS_ASSERT(false);
-        } 
         ps::parallel_for(MPs, updateRotLatLon,"updateRotationalLatitudeLongitude"); 
         pumipic::RecordTime("PolyMPO_updateRotLatLonAndXYZ2Tgt", timer.seconds());
     } 
diff --git a/src/pmpo_utils.hpp b/src/pmpo_utils.hpp
index af6e397..42971d7 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 Matrix4d;
+
 using Vec2dView = Kokkos::View<Vec2d*>;
 using Vec3dView = Kokkos::View<Vec3d*>;
 using DoubleView = Kokkos::View<double*>;
@@ -145,6 +153,147 @@ 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 Matrix4d {
+    
+  private:
+  
+    double** data_;
+    int rows_=4;
+    int cols_=4;
+   
+  public:
+			        
+    KOKKOS_INLINE_FUNCTION
+    Matrix4d(Vec4d v0, Vec4d v1, Vec4d v2, Vec4d v3){
+      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];
+      }
+    }
+
+    //Destructor    
+    KOKKOS_INLINE_FUNCTION
+    ~Matrix4d(){
+      for (int i=0; i<4; i++){
+        delete[] data_[i];
+      }
+      delete[] data_;
+    }
+        
+    //Retrieval
+    KOKKOS_INLINE_FUNCTION
+    double& operator()(int i, int j) { return data_[i][j];}
+
+    //Scalar division operator overloading
+    KOKKOS_INLINE_FUNCTION
+    Matrix4d operator/(double scalar) const {
+      Matrix4d result(Vec4d(0, 0, 0, 0), Vec4d(0, 0, 0, 0), Vec4d(0, 0, 0, 0), Vec4d(0, 0, 0, 0));
+      for (int i = 0; i < rows_; i++) {
+        for (int j = 0; j < cols_; j++) {
+          result(i, j) = data_[i][j] / scalar;
+        }
+      }
+      return result;
+    }
+
+    // Function to calculate the Frobenius norm
+    KOKKOS_INLINE_FUNCTION
+    double frobeniusNorm() const {
+      double sum = 0.0; // Initialize sum of squares
+      for (int i = 0; i < rows_; i++) {
+        for (int j = 0; j < cols_; j++) {
+          sum += data_[i][j]* data_[i][j]; // Sum of squares
+        }
+      }
+      return std::sqrt(sum); // Return the square root of the sum
+    }
+
+    // Function to calculate the trace of the matrix
+    KOKKOS_INLINE_FUNCTION
+    double trace() const {
+      double traceSum = 0.0; // Initialize sum of diagonal elements
+      for (int i = 0; i < rows_; i++) {
+        traceSum += data_[i][i]; // Sum diagonal elements
+      }
+      return traceSum;
+    }
+    
+    //Function to regularize the matrix
+    KOKKOS_INLINE_FUNCTION
+    void addToDiag(double eps) {
+      for (int i = 1; i < rows_; i++) {
+        data_[i][i]=data_[i][i]+eps;
+      }
+    }
+				        
+};
+
+
 KOKKOS_INLINE_FUNCTION
 void initArray(Vec2d* arr, int n, Vec2d fill){
     for(int i=0; i<n; i++){
@@ -152,6 +301,100 @@ void initArray(Vec2d* arr, int n, Vec2d fill){
     }
 }
 
+
+KOKKOS_INLINE_FUNCTION
+void CholeskySolve4d_UnitRHS(Matrix4d& 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(abs(A(1,1))<EPSILON || abs(A(2,2))<EPSILON || abs(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);                
+       x[2]= -(A(0,2)*x[0]+A(1,2)*x[1])/A(2,2);  
+       x[3]= -(A(0,3)*x[0]+A(1,3)*x[1]+A(2,3)*x[2])/A(3,3); 
+
+       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 QRDecomp4d(Matrix4d& A, Matrix4d& Q, Matrix4d& R){
+  int m_size=4;
+  for (int i=0; i<m_size; i++){
+      Vec4d A_column;
+      Vec4d u_column;
+      for (int l=0; l<m_size; l++){
+         A_column[l] = A(l,i);
+	 u_column[l] = A(l,i);
+      }
+
+      for (int j=0; j<i; j++){
+         Vec4d q_column;
+	 for (int l=0; l<m_size; l++)
+	   q_column[l] = Q(l,j);
+	 double r=q_column.dot(A_column);
+	 R(j,i) = r;
+         for (int k=0; k<m_size; k++) u_column[k] -= r*Q(k,j);
+      }
+      double uNorm = std::sqrt(u_column.dot(u_column));
+      R(i,i)=uNorm;
+      for (int k=0; k<m_size; k++)
+          Q(k,i)= u_column[k]/uNorm; 
+
+  }
+
+}
+
+
 KOKKOS_INLINE_FUNCTION
 void initArray(Vec3d* arr, int n, Vec3d fill){
     for(int i=0; i<n; i++){
@@ -177,7 +420,7 @@ double arcLength(Vec3d &a, Vec3d &b){
 // implement from: https://github.com/MPAS-Seaice-MPM-Project/MPAS-Seaice-MPM/blob/9bb3b4f3a09ac59fbc93b8ab317ca0fa6a6f18bb/components/mpas-framework/src/operators/mpas_geometry_utils.F#L651-L671
 //TODO:check for more effcient calculation
 KOKKOS_INLINE_FUNCTION
-double sphericalTriangleArea(Vec3d &a, Vec3d &b, Vec3d &c, double radius){
+double sphericalTriangleArea(Vec3d& a, Vec3d& b, Vec3d& c, double radius){
     double ab, bc, ca, semiperim, tanqe;
     Vec3d ablen, aclen, dlen;
 
@@ -189,8 +432,7 @@ double sphericalTriangleArea(Vec3d &a, Vec3d &b, Vec3d &c, double radius){
     tanqe = Kokkos::sqrt(Kokkos::fmax(0.0, Kokkos::tan(0.5 * semiperim) * 
                                            Kokkos::tan(0.5 * (semiperim - ab)) *
                                            Kokkos::tan(0.5 * (semiperim - bc)) * 
-                                           Kokkos::tan(0.5 * (semiperim - ca))));
-
+                                           Kokkos::tan(0.5 * (semiperim - ca)))); 
     return 4.0 * Kokkos::atan(tanqe);
 }
 
@@ -210,6 +452,31 @@ double sphericalTriangleArea2(Vec3d &a, Vec3d &b, Vec3d &c, double radius){
 //this is a lazy comparison and shouldn't be relied on beyond simple testing
 bool isEqual(double a, double b, double tol=1e-9);
 
+KOKKOS_INLINE_FUNCTION
+Vec3d xyz_from_lat_lon(double lat, double lon, double r){
+  Vec3d xyz;
+  xyz[0] = r*cos(lat)*cos(lon);
+  xyz[1] = r*cos(lat)*sin(lon);
+  xyz[2] = r*sin(lat);
+  return xyz;
+}
+
+//Rotate 90 degrees about Y axis
+KOKKOS_INLINE_FUNCTION
+Vec3d grid_rotation_backward(Vec3d& xyz_input){
+  Vec3d xyz_output;
+  xyz_output[0] =  xyz_input[2];
+  xyz_output[1] =  xyz_input[1];
+  xyz_output[2] = -xyz_input[0];
+  return xyz_output;
+}
+
+KOKKOS_INLINE_FUNCTION
+void lat_lon_from_xyz(double& lat, double& lon, Vec3d& xyz, double r){
+  lon = Kokkos::atan2(xyz[1], xyz[0]);
+  lat = Kokkos::asin(xyz[2]/r);
+} 
+
 }//namespace polyMPO end
 
 #endif
diff --git a/src/pmpo_wachspressBasis.hpp b/src/pmpo_wachspressBasis.hpp
index 13082d9..b4e4693 100644
--- a/src/pmpo_wachspressBasis.hpp
+++ b/src/pmpo_wachspressBasis.hpp
@@ -347,8 +347,8 @@ void sphericalInterpolation(MPMesh& mpMesh){
             // interpolation step
             for(int entry=0; entry<numEntries; entry++){
                 double mpValue = 0.0;
-                for(int i=0; i<= numVtx; i++){
-                    mpValue += meshField(elm2VtxConn(elm,i),entry)*basisByArea3d[i];
+                for(int i=1; i<= numVtx; i++){
+                    mpValue += meshField(elm2VtxConn(elm,i)-1,entry)*basisByArea3d[i-1];
                 }
                 mpField(mp,entry) = mpValue;
             }
diff --git a/test/calculateDisplacement.f90 b/test/calculateDisplacement.f90
index c499008..42ad0fa 100644
--- a/test/calculateDisplacement.f90
+++ b/test/calculateDisplacement.f90
@@ -1,4 +1,4 @@
-subroutine calcSurfDispIncr(mpMesh, latVertex, lonVertex, nEdgesOnCell, verticesOnCell, nVertices, sphereRadius)
+subroutine calcSurfDispIncr(mpMesh, latVertex, lonVertex, nEdgesOnCell, verticesOnCell, nVertices, sphereRadius, scale)
   use :: polympo
   use :: readMPAS
   use :: iso_c_binding
@@ -8,9 +8,16 @@ subroutine calcSurfDispIncr(mpMesh, latVertex, lonVertex, nEdgesOnCell, vertices
   real(kind=MPAS_RKIND) :: maxlon, minlon, deltaLon, sphereRadius
   integer, dimension(:), pointer :: nEdgesOnCell
   integer, dimension(:,:), pointer :: verticesOnCell
-  integer :: i, j, nVertices, nCompsDisp
+  integer :: i, j, nVertices, nCompsDisp, scale_use
   real(kind=MPAS_RKIND), dimension(:,:), pointer :: dispIncr
   type(c_ptr) :: mpMesh
+  integer, intent(in), optional :: scale
+  
+  if (present(scale)) then
+    scale_use=scale
+  else
+    scale_use=1
+  end if
 
   nCompsDisp = 2
   allocate(dispIncr(nCompsDisp,nVertices))
@@ -31,10 +38,11 @@ subroutine calcSurfDispIncr(mpMesh, latVertex, lonVertex, nEdgesOnCell, vertices
   deltaLon = maxlon - minlon
 
   do i = 1,nVertices
-    dispIncr(1,i) = sphereRadius*cos(latVertex(i))*deltaLon
+    dispIncr(1,i) = scale_use*sphereRadius*cos(latVertex(i))*deltaLon
     dispIncr(2,i) = 0.0_MPAS_RKIND
   end do
   call polympo_setMeshVtxOnSurfDispIncr(mpMesh,nCompsDisp,nVertices,c_loc(dispIncr))
 
   deallocate(dispIncr)
-end subroutine
\ No newline at end of file
+end subroutine
+
diff --git a/test/testFortranMPAdvection.f90 b/test/testFortranMPAdvection.f90
index b0d9391..b8eb5d6 100644
--- a/test/testFortranMPAdvection.f90
+++ b/test/testFortranMPAdvection.f90
@@ -54,6 +54,31 @@ subroutine runAdvectionTest(mpMesh, numPush, latVertex, lonVertex, nEdgesOnCell,
 
   end subroutine
 
+  subroutine runAdvectionTest2(mpMesh, numPush, latVertex, lonVertex, nEdgesOnCell, verticesOnCell, nVertices, sphereRadius)
+    use :: polympo
+    use :: readMPAS
+    use :: iso_c_binding
+    implicit none
+
+    type(c_ptr) :: mpMesh
+    integer :: i, numPush, nVertices
+    real(kind=MPAS_RKIND), dimension(:), pointer :: latVertex, lonVertex
+    integer, dimension(:), pointer :: nEdgesOnCell
+    integer, dimension(:,:), pointer :: verticesOnCell
+    real(kind=MPAS_RKIND) :: sphereRadius
+
+    do i = 1, numPush
+      call calcSurfDispIncr(mpMesh, latVertex, lonVertex, nEdgesOnCell, verticesOnCell, nVertices, sphereRadius)
+      call polympo_push(mpMesh)
+    end do
+
+    call calcSurfDispIncr(mpMesh, latVertex, lonVertex, nEdgesOnCell, verticesOnCell, nVertices, sphereRadius, -numPush)
+    call polympo_push(mpMesh)
+   
+  end subroutine
+
+
+
   subroutine runReconstructionTest(mpMesh, numMPs, numPush, nCells, nVertices, mp2Elm, &
                                   latVertex, lonVertex, nEdgesOnCell, verticesOnCell, sphereRadius)
     use :: polympo
@@ -193,11 +218,13 @@ program main
   real(kind=MPAS_RKIND), dimension(:), pointer :: xCell, yCell, zCell
   integer, dimension(:,:), pointer :: verticesOnCell, cellsOnCell
   integer :: numMPs, numMPsCount, numPush
-  integer, dimension(:), pointer :: mpsPerElm, mp2Elm, isMPActive
-  real(kind=MPAS_RKIND), dimension(:,:), pointer :: mpPosition, mpLatLon
+  integer, dimension(:), pointer :: mpsPerElm, mp2Elm, isMPActive, mp2Elm_new
+  real(kind=MPAS_RKIND), dimension(:,:), pointer :: mpPosition, mpLatLon, mpPositions_new, mpLatLon_new
   integer, parameter :: MP_ACTIVE = 1
   integer, parameter :: MP_INACTIVE = 0
   integer, parameter :: INVALID_ELM_ID = -1
+  real(kind=MPAS_RKIND) :: max_push_diff=0.0_MPAS_RKIND
+  real(kind=MPAS_RKIND) :: TOLERANCE_PUSH = 0.00000001_MPAS_RKIND ! 1e-8
 
   call mpi_init(ierr)
   call mpi_comm_rank(mpi_comm_handle, self, ierr)
@@ -257,9 +284,13 @@ program main
   allocate(lonCell(nCells))
   allocate(mpsPerElm(nCells))
   allocate(mp2Elm(numMPs))
+  allocate(mp2Elm_new(numMPs))
+
   allocate(isMPActive(numMPs))
   allocate(mpPosition(3,numMPs))
+  allocate(mpPositions_new(3, numMPs))
   allocate(mpLatLon(2,numMPs))
+  allocate(mpLatLon_new(2,numMPs))
 
   isMPActive = MP_ACTIVE !all active MPs and some changed below
 
@@ -329,6 +360,19 @@ program main
   call polympo_setMPRotLatLon(mpMesh,2,numMPs,c_loc(mpLatLon))
   call polympo_setMPPositions(mpMesh,3,numMPs,c_loc(mpPosition))
 
+  !Another advection test to test if material poins come back to the same position
+  call runAdvectionTest2(mpMesh, numPush, latVertex, lonVertex, nEdgesOnCell, verticesOnCell, nVertices, sphereRadius)
+  call polympo_getMPPositions(mpMesh, 3, numMPs, c_loc(mpPositions_new))
+  call polympo_getMPRotLatLon(mpMesh, 2, numMPs, c_loc(mpLatLon_new))
+    call polympo_getMPCurElmID(mpMesh, numMPS, c_loc(mp2Elm_new))
+
+  do i = 1, numMPs
+    if ( abs(mpLatLon_new(2,i)-mpLatLon(2,i)) > max_push_diff ) then
+      max_push_diff = abs(mpLatLon_new(2,i)-mpLatLon(2,i))
+    end if
+  end do
+  call assert(max_push_diff.le.TOLERANCE_PUSH , "MPs donot come back check push!")
+
   if (testType == "API") then
     call runApiTest(mpMesh, numMPs, nVertices, nCells, numPush, mpLatLon, mpPosition, xVertex, yVertex, zVertex, latVertex)
   else if (testType == "MIGRATION") then
diff --git a/test/testFortranMPReconstruction.f90 b/test/testFortranMPReconstruction.f90
index 5831291..7411af5 100644
--- a/test/testFortranMPReconstruction.f90
+++ b/test/testFortranMPReconstruction.f90
@@ -19,6 +19,7 @@ program main
   real(kind=MPAS_RKIND) :: pi = 4.0_MPAS_RKIND*atan(1.0_MPAS_RKIND)
   real(kind=MPAS_RKIND) :: TEST_VAL = 1.1_MPAS_RKIND
   real(kind=MPAS_RKIND) :: TOLERANCE = 0.0001_MPAS_RKIND
+  real(kind=MPAS_RKIND) :: TOLERANCE1 = 0.0000000001_MPAS_RKIND
   character (len=2048) :: filename
   real(kind=MPAS_RKIND), dimension(:,:), pointer :: dispIncr
   character (len=64) :: onSphere
@@ -32,7 +33,7 @@ program main
   integer, dimension(:), pointer :: mpsPerElm, mp2Elm, isMPActive
   real(kind=MPAS_RKIND), dimension(:,:), pointer :: mpPosition, mpLatLon
   real(kind=MPAS_RKIND), dimension(:,:), pointer :: mpMass, mpVel
-  real(kind=MPAS_RKIND), dimension(:), pointer :: meshVtxMass, meshElmMass
+  real(kind=MPAS_RKIND), dimension(:), pointer :: meshVtxMass, meshElmMass, meshVtxMass1
   logical :: inBound
   integer, parameter :: MP_ACTIVE = 1
   integer, parameter :: MP_INACTIVE = 0
@@ -88,6 +89,7 @@ program main
   allocate(mpMass(1,numMPs))
   allocate(mpVel(2,numMPs))
   allocate(meshVtxMass(nVertices))
+  allocate(meshVtxMass1(nVertices))
   allocate(meshElmMass(nCells))
 
   isMPActive = MP_ACTIVE !all active MPs and some changed below
@@ -99,9 +101,9 @@ program main
     j = verticesOnCell(1,i)
     mpLatLon(1,i) = latVertex(j)
     mpLatLon(2,i) = lonVertex(j) 
-    mpPosition(1,i) = xVertex(j)
-    mpPosition(2,i) = yVertex(j)
-    mpPosition(3,i) = zVertex(j)
+    mpPosition(1,i) = xCell(i)
+    mpPosition(2,i) = yCell(i)
+    mpPosition(3,i) = zCell(i)
   end do
 
   call polympo_createMPs(mpMesh,nCells,numMPs,c_loc(mpsPerElm),c_loc(mp2Elm),c_loc(isMPActive))
@@ -112,7 +114,6 @@ program main
   call polympo_setMPVel(mpMesh,2,numMPs,c_loc(mpVel))
 
   ! Test vtx reconstruction
-  
   call polympo_setReconstructionOfMass(mpMesh,0,polympo_getMeshFVtxType())
   call polympo_applyReconstruction(mpMesh)
   call polympo_getMeshVtxMass(mpMesh,nVertices,c_loc(meshVtxMass))
@@ -121,8 +122,16 @@ program main
     call assert(meshVtxMass(i) < TEST_VAL+TOLERANCE .and. meshVtxMass(i) > TEST_VAL-TOLERANCE, "Error: wrong vtx mass")
   end do
   
-  ! Test push reconstruction
+  !Test vtx order 1 reconstruction
+  call polympo_setReconstructionOfMass(mpMesh,1,polympo_getMeshFVtxType())
+  call polympo_applyReconstruction(mpMesh)
+  call polympo_getMeshVtxMass(mpMesh,nVertices,c_loc(meshVtxMass1))
+  do i = 1, nVertices
+    call assert(meshVtxMass1(i) < TEST_VAL+TOLERANCE1 .and. meshVtxMass1(i) > TEST_VAL-TOLERANCE1, "Error: wrong vtx mass lINEAR")
+  end do
+
 
+  ! Test push reconstruction
   do j = 1, 5
     call calcSurfDispIncr(mpMesh, latVertex, lonVertex, nEdgesOnCell, verticesOnCell, nVertices, sphereRadius)
     call polympo_setReconstructionOfMass(mpMesh,0,polympo_getMeshFElmType())
@@ -164,6 +173,7 @@ program main
   deallocate(mpMass)
   deallocate(mpVel)
   deallocate(meshVtxMass)
+  deallocate(meshVtxMass1)
   deallocate(meshElmMass)
 
   stop