diff --git a/main.cpp b/main.cpp
index 1845980..6f54f40 100644
--- a/main.cpp
+++ b/main.cpp
@@ -18,7 +18,7 @@ int main(int argc, char *argv[])
     std::vector<std::vector<double> > vD;
     std::string line;
     std::fstream in;
-    in.open(argc>1?argv[1]:"../shared/data/hand.pwn");
+    in.open(argc > 1 ? argv[1] : "../shared/data/hand.pwn");
     while(in)
     {
       std::getline(in, line);
diff --git a/src/fd_grad.cpp b/src/fd_grad.cpp
index 9206181..18db7e3 100644
--- a/src/fd_grad.cpp
+++ b/src/fd_grad.cpp
@@ -1,4 +1,5 @@
 #include "fd_grad.h"
+#include "fd_partial_derivative.h"
 
 void fd_grad(
   const int nx,
@@ -10,4 +11,24 @@ void fd_grad(
   ////////////////////////////////////////////////////////////////////////////
   // Add your code here
   ////////////////////////////////////////////////////////////////////////////
+  std::vector<Eigen::SparseMatrix<double>> D(3);
+  for(int i=0; i<D.size(); i++)
+    fd_partial_derivative(nx, ny, nz, h, i, D.at(i));
+
+  G.resize(D[0].rows() + D[1].rows() + D[2].rows(), D[0].cols());
+
+  typedef Eigen::Triplet<double> tuple;
+  std::vector<tuple> tupleList;
+  tupleList.reserve(2*G.rows());
+
+  //Implementation from https://stackoverflow.com/questions/41756428/concatenate-sparse-matrix-eigen
+  //for stacking sparse matrix
+  int offset[3] = { 0, D[0].rows(), D[0].rows()+D[1].rows()};
+  for(int i=0; i<3; i++)
+    for (int k = 0; k < D[i].outerSize(); ++k)
+      for (Eigen::SparseMatrix<double>::InnerIterator it(D.at(i), k); it; ++it)
+        tupleList.push_back(tuple(offset[i]+it.row(), it.col(), it.value()));
+
+  G.setFromTriplets(tupleList.begin(), tupleList.end());
 }
+
diff --git a/src/fd_interpolate.cpp b/src/fd_interpolate.cpp
index 2a32675..ac9a198 100644
--- a/src/fd_interpolate.cpp
+++ b/src/fd_interpolate.cpp
@@ -1,4 +1,6 @@
 #include "fd_interpolate.h"
+#include <math.h>
+#include <iostream>
 
 void fd_interpolate(
   const int nx,
@@ -12,4 +14,36 @@ void fd_interpolate(
   ////////////////////////////////////////////////////////////////////////////
   // Add your code here
   ////////////////////////////////////////////////////////////////////////////
+  // number of input points
+  const int n = P.rows();
+
+  typedef Eigen::Triplet<double> tuple;
+  std::vector<tuple> tupleList;
+  tupleList.reserve(8*n);
+
+  for (int m = 0; m < n; m++)
+  {
+    //Shift the volume to the origin
+    double X = P(m, 0) - corner(0);
+    double Y = P(m, 1) - corner(1);
+    double Z = P(m, 2) - corner(2);
+
+    //calculate grid coordinate
+    int i = (int)trunc(X / h);
+    int j = (int)trunc(Y / h);
+    int k = (int)trunc(Z / h);
+
+    //Normalize the single grid size to one
+    double x = (X - i*h) / h;
+    double y = (Y - j*h) / h;
+    double z = (Z - k*h) / h;
+
+    //Using the equations on http://paulbourke.net/miscellaneous/interpolation/
+    //to calculate the weight of each vertices on the unit grid cube
+    for(int r=0;r<=1;r++)
+      for (int s = 0; s <= 1; s++)
+        for (int t = 0; t <= 1; t++)
+          tupleList.push_back(tuple(m, (i+r)+(j+s)*nx+(k+t)*nx*ny, (r*x+(1-r)*(1-x))*(s*y+(1-s)*(1-y))*(t*z+(1-t)*(1-z))));
+  }
+  W.setFromTriplets(tupleList.begin(), tupleList.end());
 }
diff --git a/src/fd_partial_derivative.cpp b/src/fd_partial_derivative.cpp
index c3c4deb..8e70045 100644
--- a/src/fd_partial_derivative.cpp
+++ b/src/fd_partial_derivative.cpp
@@ -11,4 +11,21 @@ void fd_partial_derivative(
   ////////////////////////////////////////////////////////////////////////////
   // Add your code here
   ////////////////////////////////////////////////////////////////////////////
+  int offset[3] = { 0 };
+  offset[dir] = 1;
+  D.resize((nx-offset[0])*(ny- offset[1])*(nz-offset[2]), nx*ny*nz);
+  //From tutorial https://eigen.tuxfamily.org/dox/group__SparseQuickRefPage.html
+  //and https://stackoverflow.com/questions/14697375/assigning-a-sparse-matrix-in-eigen
+  typedef Eigen::Triplet<double> tuple;
+  std::vector<tuple> tupleList;
+  tupleList.reserve(2 * D.rows());
+  double h_inv = 1 / h;
+  for (int i = 0; i < nx-offset[0]; i++) 
+    for (int j = 0; j < ny-offset[1]; j++)
+      for (int k = 0; k < nz-offset[2]; k++)
+      {
+        tupleList.push_back(tuple(i + j*(nx- offset[0])+k*(nx- offset[0])*(ny- offset[1]),  i             +  j            *nx +  k            *nx*ny, -h_inv));
+        tupleList.push_back(tuple(i + j*(nx- offset[0])+k*(nx- offset[0])*(ny- offset[1]), (i+ offset[0]) + (j+ offset[1])*nx + (k+ offset[2])*nx*ny,  h_inv));
+      }
+  D.setFromTriplets(tupleList.begin(), tupleList.end());
 }
diff --git a/src/poisson_surface_reconstruction.cpp b/src/poisson_surface_reconstruction.cpp
index 4fe7c1e..6683c7e 100644
--- a/src/poisson_surface_reconstruction.cpp
+++ b/src/poisson_surface_reconstruction.cpp
@@ -1,7 +1,15 @@
 #include "poisson_surface_reconstruction.h"
 #include <igl/copyleft/marching_cubes.h>
 #include <algorithm>
+/////////////////////////////////////////
+//My Code
+#include <Eigen/Sparse>
+#include <iostream>
+#include "fd_interpolate.h"
+#include "fd_grad.h"
 
+//End of My Code
+//////////////////////////////////////////
 void poisson_surface_reconstruction(
     const Eigen::MatrixXd & P,
     const Eigen::MatrixXd & N,
@@ -31,22 +39,54 @@ void poisson_surface_reconstruction(
   // Compute positions of grid nodes
   Eigen::MatrixXd x(nx*ny*nz, 3);
   for(int i = 0; i < nx; i++) 
-  {
     for(int j = 0; j < ny; j++)
-    {
       for(int k = 0; k < nz; k++)
       {
          // Convert subscript to index
          const auto ind = i + nx*(j + k * ny);
          x.row(ind) = corner + h*Eigen::RowVector3d(i,j,k);
       }
-    }
-  }
   Eigen::VectorXd g = Eigen::VectorXd::Zero(nx*ny*nz);
 
   ////////////////////////////////////////////////////////////////////////////
   // Add your code here
   ////////////////////////////////////////////////////////////////////////////
+  // Compute grad matrix G
+  Eigen::SparseMatrix<double> G;
+  fd_grad(nx, ny, nz, h, G);
+
+  std::vector<Eigen::SparseMatrix<double>> W_xyz(3);
+  std::vector<Eigen::VectorXd> v_xyz(3);
+  for (int i = 0; i < W_xyz.size(); i++)
+  {
+    double offset[3] = { 0 };
+    offset[i] = 1;
+    W_xyz.at(i).resize(n, (nx - offset[0])*(ny - offset[1])*(nz - offset[2]));
+    fd_interpolate((nx - offset[0]), (ny - offset[1]), (nz - offset[2]), h, corner, P, W_xyz.at(i));
+    v_xyz.at(i)= W_xyz.at(i).transpose()*N.col(i);
+  }
+
+  //from https://stackoverflow.com/questions/25691324/how-to-concatenate-vectors-in-eigen
+  Eigen::VectorXd v(v_xyz[0].rows()+ v_xyz[1].rows()+ v_xyz[2].rows());
+  v << v_xyz[0], v_xyz[1], v_xyz[2];
+
+  Eigen::SparseMatrix<double> GG = G.transpose()*G;
+  Eigen::MatrixXd Gv = G.transpose()*v;
+
+  //from https://eigen.tuxfamily.org/dox/classEigen_1_1ConjugateGradient.html
+  Eigen::ConjugateGradient<Eigen::SparseMatrix<double>, Eigen::UpLoType::Lower | Eigen::UpLoType::Upper> cg;
+  cg.compute(GG);
+  g = cg.solve(Gv);
+  std::cout << "#iterations:     " << cg.iterations() << std::endl;
+  std::cout << "estimated error: " << cg.error() << std::endl;
+
+  Eigen::SparseMatrix<double> W(n, nx*ny*nz);
+  fd_interpolate(nx,ny,nz, h, corner, P, W);
+  auto one =Eigen::VectorXd::Ones(n)/(double)n; 
+  double sigma = one.transpose()*(W*g);
+  //from https://stackoverflow.com/questions/35688805/eigen-subtracting-a-scalar-from-a-vector
+  g = g.array() - sigma*1.45;//Shrink it a bit more so that the fingers do not stick together.
+  std::cout <<"sigma:"<< sigma << std::endl;
 
   ////////////////////////////////////////////////////////////////////////////
   // Run black box algorithm to compute mesh from implicit function: this
@@ -54,3 +94,4 @@ void poisson_surface_reconstruction(
   ////////////////////////////////////////////////////////////////////////////
   igl::copyleft::marching_cubes(g, x, nx, ny, nz, V, F);
 }
+