diff --git a/src/seam-carving.cpp b/src/seam-carving.cpp
index db04ff2..4817c2d 100644
--- a/src/seam-carving.cpp
+++ b/src/seam-carving.cpp
@@ -19,11 +19,18 @@ int min(int a, int b) {
   return a < b ? a : b;
 }
 
-/** e_1 energy */
-std::vector<unsigned char> energy_e1(unsigned char* source, int width, int height, int nbChannels) {
-  std::vector<unsigned char> energy(width*height);
+void export_image(const char* filename, const void* data, int width, int height, int nbChannels) {
+  if (!silent) std::cout << "Exporting to \"" << filename << "\".." << std::endl;
+  int errcode = stbi_write_png(filename, width, height, nbChannels, data, nbChannels*width);
+  if (!errcode) {
+    std::cout<<"Error while exporting the resulting image."<<std::endl;
+    exit(errcode);
+  }
+}
 
-  for (auto i=0; i < width*height; i++) { energy[i] = 0; }
+/** e_1 energy */
+std::vector<unsigned char> energy_e1(std::vector<unsigned char> source, int width, int height, int nbChannels) {
+  std::vector<unsigned char> energy(width*height);
 
   for(auto i=0 ; i < width ; ++i) {
     for(auto j=0; j < height; ++j) {
@@ -32,7 +39,8 @@ std::vector<unsigned char> energy_e1(unsigned char* source, int width, int heigh
       auto indexPixel_down  = (j+1 < height) ? nbChannels*(width*(j+1)+i) : indexPixel;
       auto indexPixel_left  = (i-1 > 0)      ? nbChannels*(width*j+(i-1)) : indexPixel;
       auto indexPixel_right = (i+1 < width)  ? nbChannels*(width*j+(i+1)) : indexPixel;
-
+    
+      energy[width*j+i] = 0;
       for (auto ch=0; ch < nbChannels; ch++) {
         energy[width*j+i] += (
             fabs((float)source[indexPixel_up+ch] - source[indexPixel+ch])
@@ -47,7 +55,8 @@ std::vector<unsigned char> energy_e1(unsigned char* source, int width, int heigh
   return energy;
 }
 
-std::vector<int> optimal_vertical_seams(std::vector<unsigned char> energy, int width, int height, int nbSeams) {
+/** Given the energy value, returns the optimal vertical seam */
+std::vector<int> optimal_vertical_seam(std::vector<unsigned char> energy, int width, int height) {
   std::vector<unsigned char> dyn_energy(width*height);
 
   //* Find an end of the minimal connected vertical/horizontal seam
@@ -61,7 +70,6 @@ std::vector<int> optimal_vertical_seams(std::vector<unsigned char> energy, int w
       int bot_left = (i > 0 && j > 0) ? dyn_energy[width*(i-1)+(j-1)] : INT_MAX;
       int bot_right = (i > 0 && j+1 < width) ? dyn_energy[width*(i-1)+(j+1)] : INT_MAX;
 
-
       dyn_energy[width*i+j] = min(
         bot_center,
         min(
@@ -72,45 +80,67 @@ std::vector<int> optimal_vertical_seams(std::vector<unsigned char> energy, int w
     }
   }
 
-  std::vector<int> result(height*nbSeams);
-
-  // To find the nbSeams largest points, we sort the pairs (dyn_energy[idx, height-1], idx)
-  std::vector<std::pair<int, int>> seamEnds(width);
+  std::vector<int> result(height);
+  // Find the seam end
+  int min_idx = -1;
+  int min_val = INT_MAX;
   for (auto j=0; j < width; j++) {
-    seamEnds[j] = {dyn_energy[(height-1)*width+j], j};
-  }
-  std::sort(seamEnds.begin(), seamEnds.end(),
-      [](std::pair<int, int> a, std::pair<int, int> b) {return a.first > b.first; });
-  for (auto seam=0; seam < nbSeams; seam++) {
-    result[height*(seam+1)-1] = seamEnds[seam].second;
+    if (min_val > dyn_energy[width*(height-1)+j]) {
+      min_idx = j;
+      min_val = dyn_energy[width*(height-1)+j];
+    }
   }
+  result[height-1] = min_idx;
 
   //* Backtracking to find the path
-  for (auto seam=0; seam < nbSeams; seam++) {
-    int min_idx = result[height*(seam+1)-1];
-    int min_val = dyn_energy[(height-1)*width+min_idx];
-
-    for (auto i=height-1; i > 0; i--) {
-      // We want to find either (bot_l, bot_c, bot_r) with dyn_energy[.] = min_val - energy[cur]
-      int objective_energy = min_val - energy[i*width+min_idx];
-      
-      if (dyn_energy[(i-1)+height*min_idx] == objective_energy) {
-        // min_idx does not change
-        min_val = dyn_energy[(i-1)+height*min_idx];
-      } else if (min_idx > 0 && dyn_energy[(i-1)+height*(min_idx-1)] == objective_energy) {
-        min_val = dyn_energy[(i-1)+height*(min_idx - 1)];
-      min_idx = min_idx - 1;
-    } else if (min_idx+1 < width && dyn_energy[(i-1)+height*(min_idx+1)] == objective_energy) {
-      min_val = dyn_energy[(i-1)+height*(min_idx + 1)];
-      min_idx = min_idx + 1;
-    }
-    result[height*seam+i-1] = min_idx;
-    }
+  for (auto i=height-1; i > 0; i--) {
+    // We want to find either (bot_l, bot_c, bot_r) with dyn_energy[.] = min_val - energy[cur]
+    int objective_energy = min_val - energy[i*width+min_idx];
+    
+    if (dyn_energy[(i-1)+height*min_idx] == objective_energy) {
+      // min_idx does not change
+      min_val = dyn_energy[(i-1)+height*min_idx];
+    } else if (min_idx > 0 && dyn_energy[(i-1)+height*(min_idx-1)] == objective_energy) {
+      min_val = dyn_energy[(i-1)+height*(min_idx - 1)];
+    min_idx = min_idx - 1;
+  } else if (min_idx+1 < width && dyn_energy[(i-1)+height*(min_idx+1)] == objective_energy) {
+    min_val = dyn_energy[(i-1)+height*(min_idx + 1)];
+    min_idx = min_idx + 1;
+  }
+  result[i-1] = min_idx;
   }
 
   return result;
 }
 
+/** Carves an image by one vertical seam. Returns the optimal seam used */
+std::vector<int> carving_step_vertical(const std::vector<unsigned char> source, std::vector<unsigned char> &output, int width, int height, int nbChannels) {
+  
+  std::vector<unsigned char> energy = energy_e1(source, width, height, nbChannels);
+  std::vector<int> opt_seam = optimal_vertical_seam(energy, width, height);
+  std::vector<bool> blacklist(width*height);
+
+  for (auto k=0; k < width*height; k++) { blacklist[k] = false; }
+  for (auto i=0; i < height; i++) {
+    blacklist[i*width+opt_seam[i]] = true;
+  }
+
+  int outWidth = width-1;
+
+  for (auto i=0; i < height; i++) {
+    int cur_j = 0;
+    for (auto j=0; cur_j < outWidth && j < width; j++) {
+      if (!blacklist[i*width+j]) {
+        output[3*(i*(width-1)+cur_j)] =   source[3*(i*width+j)];
+        output[3*(i*(width-1)+cur_j)+1] = source[3*(i*width+j)+1];
+        output[3*(i*(width-1)+cur_j)+2] = source[3*(i*width+j)+2];
+        cur_j++;
+      }
+    }
+  }
+  return opt_seam;
+}
+
 
 int main(int argc, char **argv) {
   CLI::App app{"seam-carving"};
@@ -129,78 +159,70 @@ int main(int argc, char **argv) {
   //Image loading
   int width, height, nbChannels;
   unsigned char *source = stbi_load(sourceImage.c_str(), &width, &height, &nbChannels, 0);
+  nbSeams = min(nbSeams, width);
 
   if (nbChannels < 3) { // TODO : really ?
     std::cout<< "Input images must be RGB images."<<std::endl;
     exit(1);
   }
 
-  nbSeams = min(nbSeams, width);
-  int outChannels = nbChannels;
-  int outHeight = height;
-  int outWidth = width;
-  std::vector<unsigned char> output;
+  if (test_energy) { // Vertical seam carving. Prepare
+    std::vector<unsigned char> output(width*height*3); // Final output (initial energy function + where we set removed pixels red)
+    std::vector<unsigned char> carve_output(width*height*3); // Receives at each step the newly carved image
+    std::vector<unsigned char> source_img(width*height*nbChannels); // Contains at each step the carved image
+    std::vector<bool> complete_blacklist(width*height); // Contains all removed pixels
 
-  if (test_energy) {
-    //outChannels = 1;
-    std::vector<unsigned char> energy = energy_e1(source, width, height, nbChannels);
-    std::vector<int> opt_seam = optimal_vertical_seams(energy, width, height, nbSeams);
+    for (auto k=0; k < width*height; k++) { complete_blacklist[k] = false; }
+    for (auto i=0; i < width*height*nbChannels; i++) { source_img[i] = source[i]; }
 
-    std::vector<unsigned char> output2(width*height*3);
-    for (auto i=0; i < width*height; i++) {
-      output2[3*i] = energy[i];
-      output2[3*i+1] = energy[i];
-      output2[3*i+2] = energy[i];
+    std::vector<unsigned char> ini_energy = energy_e1(source_img, width, height, nbChannels);
+
+    //* Prepare final output
+    for (auto k=0; k < width*height; k++) {
+      //output[3*k] = source_img[3*k]/3; //* Uncomment if you prefer to see darkened source image
+      //output[3*k+1] = source_img[3*k+1]/3;
+      //output[3*k+2] = source_img[3*k+2]/3;
+      output[3*k] = ini_energy[k];
+      output[3*k+1] = ini_energy[k];
+      output[3*k+2] = ini_energy[k];
     }
+
+    int curWidth = width;
     for (auto seam=0; seam < nbSeams; seam++) {
+      std::cout << seam << std::endl;
+      std::vector<int> opt_seam = carving_step_vertical(source_img, carve_output, curWidth, height, nbChannels); // Perform a carving step
+      std::copy(carve_output.begin(), carve_output.end(), source_img.begin()); // Copy output to next input
+      curWidth--;
+
       for (auto i=0; i < height; i++) {
-        output2[3*(i*width+opt_seam[seam*height+i])] = 255;
-        output2[3*(i*width+opt_seam[seam*height+i])+1] = 0;
-        output2[3*(i*width+opt_seam[seam*height+i])+2] = 0;
-      }
-    }
-    output=output2;
-  } else {
-    std::vector<unsigned char> energy = energy_e1(source, width, height, nbChannels);
-    std::vector<int> opt_seam = optimal_vertical_seams(energy, width, height, nbSeams);
-
-    std::vector<bool> blacklist(width*height);
-
-    for (auto k=0; k < width*height; k++) { blacklist[k] = false; }
-    for (auto seam=0; seam < nbSeams; seam++) {
-      bool overlap = false; // Check if 2 seams use similar pixels
-      for (auto i=0; i < height; i++) {
-        overlap = overlap || blacklist[i*width+opt_seam[seam*height+i]];
-      }
-      if (overlap) { nbSeams--; continue;}
-      for (auto i=0; i < height; i++) {
-        blacklist[i*width+opt_seam[seam*height+i]] = true;
-      }
-    }
-
-    outWidth = width-nbSeams; //! something is weird here
-
-    std::vector<unsigned char> output_l(outWidth*height*3);
-    for (auto i=0; i < height; i++) {
-      int cur_j = 0;
-      for (auto j=0; cur_j < outWidth && j < width; j++) {
-        if (!blacklist[i*width+j]) {
-          output_l[3*(i*outWidth+cur_j)] =   source[3*(i*width+j)];
-          output_l[3*(i*outWidth+cur_j)+1] = source[3*(i*width+j)+1];
-          output_l[3*(i*outWidth+cur_j)+2] = source[3*(i*width+j)+2];
-          cur_j++;
+        int cur_j = 0; // cur_j is the index relative to the current carved image. j is absolute in the source image
+        int j;
+        for (j=0; j < width && cur_j < opt_seam[i]; j++) {
+          if (!complete_blacklist[i*width+j]) { cur_j++; }
         }
+        assert(cur_j == opt_seam[i]); // Else, j == width and cur_j is not in the source image..
+
+        complete_blacklist[i*width+j] = true;
+        output[3*(i*width+j)] = 255;
       }
     }
-    output = output_l;
-  }
 
-  //Final export
-  if (!silent) std::cout<<"Exporting.."<<std::endl;
-  int errcode = stbi_write_png(outputImage.c_str(), outWidth, outHeight, outChannels, output.data(), outChannels*outWidth);
-  if (!errcode) {
-    std::cout<<"Error while exporting the resulting image."<<std::endl;
-    exit(errcode);
+    export_image(outputImage.c_str(), output.data(), width, height, nbChannels);
+
+  } else { // Standard vertical seam carving
+    int outWidth = width;
+    std::vector<unsigned char> output(width*height*3);
+    std::vector<unsigned char> vect_source(width*height*nbChannels);
+
+    for (auto i=0; i < width*height*nbChannels; i++) { vect_source[i] = source[i]; }
+
+    for (auto seam=0; seam < nbSeams; seam++) {
+      std::cout << seam << std::endl;
+      carving_step_vertical(vect_source, output, outWidth, height, nbChannels);
+      std::copy(output.begin(), output.end(), vect_source.begin());
+      outWidth--;
+    }
+    export_image(outputImage.c_str(), vect_source.data(), outWidth, height, nbChannels);
   }
 
   stbi_image_free(source);