diff --git a/src/main.cpp b/src/main.cpp
index 5d98425..e915f96 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -8,6 +8,8 @@
 #include <gdal/ogr_geometry.h>
 #include <iostream>
 #include <ostream>
+#include <sys/types.h>
+#include <tuple>
 #include <vector>
 #include <cstdint>
 #include <gdal/gdal.h>
@@ -69,10 +71,8 @@ std::vector<std::vector<Point>> create_lines(HeightMap* heightmap, int interval)
             for (int j = 0; j < points.size(); j++){
                 for (int k = 0; k < points.size(); k++){
                     Point* candidate = &points[k];
-
-
                     if (!candidate->allocated) {
-                        if (candidate->x == x +1 && candidate->y == y && is_in(current_case, east)) {
+                        if (candidate->x == x +1 && candidate->y == y /*&& is_in(current_case, east)*/) {
                             candidate->allocated = true;
                             x = candidate->x;
                             y = candidate->y;
@@ -81,7 +81,7 @@ std::vector<std::vector<Point>> create_lines(HeightMap* heightmap, int interval)
                             points_allocated++;
                             break;
                         }
-                        else if (candidate->x == x -1 && candidate->y == y && is_in(current_case, west)) {
+                        else if (candidate->x == x -1 && candidate->y == y /*&& is_in(current_case, west)*/) {
                             x = candidate->x;
                             y = candidate->y;
                             current_case = candidate->mscase;
@@ -90,7 +90,7 @@ std::vector<std::vector<Point>> create_lines(HeightMap* heightmap, int interval)
                             points_allocated++;
                             break;
                         }
-                        else if (candidate->x == x && candidate->y == y + 1 && is_in(current_case, north)) {
+                        else if (candidate->x == x && candidate->y == y + 1 /*&& is_in(current_case, north)*/) {
                             x = candidate->x;
                             y = candidate->y;
                             current_case = candidate->mscase;
@@ -99,7 +99,7 @@ std::vector<std::vector<Point>> create_lines(HeightMap* heightmap, int interval)
                             points_allocated++;
                             break;
                         }
-                        else if (candidate->x == x && candidate->y == y - 1 && is_in(current_case, south)) {
+                        else if (candidate->x == x && candidate->y == y - 1 /*&& is_in(current_case, south)*/) {
                             x = candidate->x;
                             y = candidate->y;
                             current_case = candidate->mscase;
@@ -120,6 +120,7 @@ std::vector<std::vector<Point>> create_lines(HeightMap* heightmap, int interval)
                         if (!candidate->allocated)
                         {
                             line.push_back(*candidate);
+                            candidate->allocated = true;
                             x = candidate->x;
                             y = candidate->y;
                             current_case = candidate->mscase;
@@ -150,20 +151,29 @@ std::tuple<double, double> local_to_projected(double* geotransform, int x, int y
 constexpr std::tuple<double, double, double, double> marching_squares_lookup(int mcase)
 {
     switch (mcase) {
-        case 1: return {0, 0.5, 0.5, 0};
-        case 2: return {1, 0.5, 0.5, 0};
-        case 3: return {0, 0.5, 1, 0.5};
-        case 4: return {0.5, 1, 1, 0.5};
-        case 5: return {0, 0, 0, 0}; //FIXME
-        case 6: return {0.5, 1, 0.5, 0};
-        case 7: return {0, 0.5, 0.5, 1};
-        case 8: return {0, 0.5, 0.5, 1};
-        case 9: return {0.5, 1, 0.5, 0};
-        case 10: return {0, 0, 0, 0}; //FIXME
-        case 11: return {0.5, 1, 1, 0.5};
-        case 12: return {0, 0.5, 1, 0.5};
-        case 13: return {0.5, 0, 1, 0.5};
-        case 14: return {0, 0.5, 0.5, 0};
+        case 1:
+        case 14:
+            return {0, 0.5, 0.5, 0};
+        case 2: 
+            return {0.5, 0, 1, 0.5};
+        case 3: 
+        case 12:
+            return {0, 0.5, 1, 0.5};
+        case 4: 
+        case 11:
+            return {0.5, 1, 1, 0.5};
+        case 5: 
+        case 10:
+            return {0, 0, 0, 0}; //FIXME
+        case 6: 
+        case 9:
+            return {0.5, 1, 0.5, 0};
+        case 7: 
+        case 8:
+            return {0, 0.5, 0.5, 1};
+        case 13:
+            return {1, 0.5, 0.5, 0};
+        
     };
 }
 void write_output_file(std::vector<std::vector<Point>> all_points, const char *filepath, HeightMap* heightmap)
@@ -220,14 +230,41 @@ void write_output_file(std::vector<std::vector<Point>> all_points, const char *f
         feature->SetField( "Name", j );
 
         std::vector<Point> points = all_points[j];
+        
+        bool left_to_right = true;
+        
+        if ((points[0].y - points[1].y) < 0)
+            left_to_right = true;
+        else
+            left_to_right = false;
 
         for (int k = 0; k < points.size(); k++)
         {
+            bool left_to_right = true;
+        
+            if ((points[k].x - points[k-1].x) < 0)
+                left_to_right = false;
+            bool top_to_bottom = true;
+            if ((points[k].y - points[k-1].y) < 0)
+                left_to_right = false;
+
+
+
             int x_raw = points[k].x;
             int y_raw = points[k].y;
             auto [x, y] = local_to_projected(heightmap->geotransform, x_raw, y_raw);
-            //auto [x_bl, y_bl, x_tr, y_tr] = marching_squares_lookup(*(cellmap->cells + i) );
-            geometry->setPoint(geometry->getNumPoints(),x ,y );
+
+            auto [x1, y1, x2, y2] = marching_squares_lookup(points[k].mscase);
+            
+            if (left_to_right or !top_to_bottom)
+            {
+                geometry->setPoint(geometry->getNumPoints(),x + (x1/6) ,y + (y1/6));
+                geometry->setPoint(geometry->getNumPoints(),x + (x2/6) ,y + (y2/6));
+            }
+            else {
+                geometry->setPoint(geometry->getNumPoints(),x + (x2/6) ,y + (y2/6));
+                geometry->setPoint(geometry->getNumPoints(),x + (x1/6) ,y + (y1/6));
+            }
         }
 
         if ( feature->SetGeometry(geometry) != OGRERR_NONE)