diff --git a/src/fd_grad.cpp b/src/fd_grad.cpp
index 9206181..c84c2e5 100644
--- a/src/fd_grad.cpp
+++ b/src/fd_grad.cpp
@@ -1,4 +1,6 @@
 #include "fd_grad.h"
+#include "fd_partial_derivative.h"
+#include <vector>
 
 void fd_grad(
   const int nx,
@@ -7,7 +9,35 @@ void fd_grad(
   const double h,
   Eigen::SparseMatrix<double> & G)
 {
-  ////////////////////////////////////////////////////////////////////////////
-  // Add your code here
-  ////////////////////////////////////////////////////////////////////////////
+  int rows = (nx-1)*ny*nz + nx*(ny-1)*nz + nx*ny*(nz-1);
+  int cols = nx*ny*nz;
+  G.resize(rows, cols);
+  
+  Eigen::SparseMatrix<double> Dx, Dy, Dz;
+  fd_partial_derivative(nx, ny, nz, h, 0, Dx);
+  fd_partial_derivative(nx, ny, nz, h, 1, Dy);
+  fd_partial_derivative(nx, ny, nz, h, 2, Dz);
+  
+  // ideally, there would be a helper for concatenating sparse matrices
+  
+  typedef Eigen::Triplet<double> T;
+  std::vector<T> tripletList;
+  
+  for (int i = 0; i < Dx.outerSize(); i++) {
+    for (Eigen::SparseMatrix<double>::InnerIterator it(Dx,i); it; ++it) {
+      tripletList.push_back(T(it.row(), it.col(), it.value()));
+    }
+  }
+  for (int i = 0; i < Dy.outerSize(); i++) {
+    for (Eigen::SparseMatrix<double>::InnerIterator it(Dy,i); it; ++it) {
+      tripletList.push_back(T(Dx.rows() + it.row(), it.col(), it.value()));
+    }
+  }
+  for (int i = 0; i < Dz.outerSize(); i++) {
+    for (Eigen::SparseMatrix<double>::InnerIterator it(Dz,i); it; ++it) {
+      tripletList.push_back(T(Dx.rows() + Dy.rows() + it.row(), it.col(), it.value()));
+    }
+  }
+   
+  G.setFromTriplets(tripletList.begin(), tripletList.end());
 }
diff --git a/src/fd_interpolate.cpp b/src/fd_interpolate.cpp
index 2a32675..9cad00b 100644
--- a/src/fd_interpolate.cpp
+++ b/src/fd_interpolate.cpp
@@ -1,4 +1,8 @@
 #include "fd_interpolate.h"
+#include <math.h>
+#include <vector>
+
+#define IND(i,j,k) i + nx*(j + (k) * ny)
 
 void fd_interpolate(
   const int nx,
@@ -9,7 +13,44 @@ void fd_interpolate(
   const Eigen::MatrixXd & P,
   Eigen::SparseMatrix<double> & W)
 {
-  ////////////////////////////////////////////////////////////////////////////
-  // Add your code here
-  ////////////////////////////////////////////////////////////////////////////
+  W.resize(P.rows(), nx*ny*nz);
+  
+  typedef Eigen::Triplet<double> T;
+  std::vector<T> tripletList;
+  
+  for (int i = 0; i < W.rows(); i++) {
+  
+    double px = P(i,0);
+    double py = P(i,1);
+    double pz = P(i,2);
+    
+    // obtain corresponding (non-integer) grid coordinates
+    double x = (px - corner[0]) / h;
+    double y = (py - corner[1]) / h;
+    double z = (pz - corner[2]) / h;
+    
+    // obtain the coordinates involved in points of surrounding cube
+    int x_l = floor(x);
+    int x_r = ceil(x);
+    int y_l = floor(y);
+    int y_r = ceil(y);
+    int z_l = floor(z);
+    int z_r = ceil(z);
+    
+    double x_d = x - x_l;
+    double y_d = y - y_l;
+    double z_d = z - z_l;
+    
+    // set the weights for each of the eight surrounding points
+    tripletList.push_back(T(i, IND(x_l,y_l,z_l), (1 - x_d) * (1 - y_d) * (1 - z_d)));
+    tripletList.push_back(T(i, IND(x_l,y_l,z_r), (1 - x_d) * (1 - y_d) * z_d));
+    tripletList.push_back(T(i, IND(x_l,y_r,z_l), (1 - x_d) * y_d * (1 - z_d)));
+    tripletList.push_back(T(i, IND(x_l,y_r,z_r), (1 - x_d) * y_d * z_d));
+    tripletList.push_back(T(i, IND(x_r,y_l,z_l), x_d * (1 - y_d) * (1 - z_d)));
+    tripletList.push_back(T(i, IND(x_r,y_l,z_r),  x_d * (1 - y_d) * z_d));
+    tripletList.push_back(T(i, IND(x_r,y_r,z_l), x_d * y_d * (1 - z_d)));
+    tripletList.push_back(T(i, IND(x_r,y_r,z_r), x_d * y_d * z_d));
+  }
+  
+  W.setFromTriplets(tripletList.begin(), tripletList.end());
 }
diff --git a/src/fd_partial_derivative.cpp b/src/fd_partial_derivative.cpp
index c3c4deb..10f8bbc 100644
--- a/src/fd_partial_derivative.cpp
+++ b/src/fd_partial_derivative.cpp
@@ -1,4 +1,5 @@
 #include "fd_partial_derivative.h"
+#include <vector>
 
 void fd_partial_derivative(
   const int nx,
@@ -8,7 +9,26 @@ void fd_partial_derivative(
   const int dir,
   Eigen::SparseMatrix<double> & D)
 {
-  ////////////////////////////////////////////////////////////////////////////
-  // Add your code here
-  ////////////////////////////////////////////////////////////////////////////
+  int nx_p = nx - (dir == 0);
+  int ny_p = ny - (dir == 1);
+  int nz_p = nz - (dir == 2);
+
+  D.resize(nx_p*ny_p*nz_p, nx*ny*nz);
+  
+  typedef Eigen::Triplet<double> T;
+  std::vector<T> tripletList;
+  
+  for (int i = 0; i < nx_p; i++) {
+    for (int j = 0; j < ny_p; j++) {
+      for (int k = 0; k < nz_p; k++) {
+        int row = i + nx_p*(j + k * ny_p);
+        int left_ind = i + nx*(j + k * ny);
+        int right_ind = i + (dir == 0) + nx*(j + (dir == 1) + (k + (dir == 2))* ny);
+        tripletList.push_back(T(row,left_ind,-1/h));
+        tripletList.push_back(T(row,right_ind,1/h));
+      }
+    }
+  }
+  
+  D.setFromTriplets(tripletList.begin(), tripletList.end());
 }
diff --git a/src/poisson_surface_reconstruction.cpp b/src/poisson_surface_reconstruction.cpp
index 4fe7c1e..75d32d4 100644
--- a/src/poisson_surface_reconstruction.cpp
+++ b/src/poisson_surface_reconstruction.cpp
@@ -1,4 +1,6 @@
 #include "poisson_surface_reconstruction.h"
+#include "fd_interpolate.h"
+#include "fd_grad.h"
 #include <igl/copyleft/marching_cubes.h>
 #include <algorithm>
 
@@ -10,6 +12,7 @@ void poisson_surface_reconstruction(
 {
   ////////////////////////////////////////////////////////////////////////////
   // Construct FD grid, CONGRATULATIONS! You get this for free!
+  // Thanks!
   ////////////////////////////////////////////////////////////////////////////
   // number of input points
   const int n = P.rows();
@@ -43,11 +46,26 @@ void poisson_surface_reconstruction(
     }
   }
   Eigen::VectorXd g = Eigen::VectorXd::Zero(nx*ny*nz);
+  
+  Eigen::SparseMatrix<double> W, Wx, Wy, Wz, G;
+  fd_interpolate(nx, ny, nz, h, corner, P, W);
+  fd_interpolate(nx-1, ny, nz, h, corner + (h/2)*Eigen::RowVector3d(1,0,0), P, Wx);
+  fd_interpolate(nx, ny-1, nz, h, corner + (h/2)*Eigen::RowVector3d(0,1,0), P, Wy);
+  fd_interpolate(nx, ny, nz-1, h, corner + (h/2)*Eigen::RowVector3d(0,0,1), P, Wz);
+  fd_grad(nx, ny, nz, h, G);
 
-  ////////////////////////////////////////////////////////////////////////////
-  // Add your code here
-  ////////////////////////////////////////////////////////////////////////////
+  Eigen::VectorXd v((nx-1)*ny*nz + nx*(ny-1)*nz + nx*ny*(nz-1));
+  v << Wx.transpose() * N.col(0),
+       Wy.transpose() * N.col(1),
+       Wz.transpose() * N.col(2);
+
+  Eigen::ConjugateGradient<Eigen::SparseMatrix<double>> solver;
+  solver.compute(G.transpose() * G);
+  g = solver.solve(G.transpose() * v);
 
+  double sigma = (1./n) * Eigen::RowVectorXd::Ones(n) * W * g;
+  g -= sigma * Eigen::VectorXd::Ones(nx*ny*nz);
+  
   ////////////////////////////////////////////////////////////////////////////
   // Run black box algorithm to compute mesh from implicit function: this
   // function always extracts g=0, so "pre-shift" your g values by -sigma