diff --git a/src/contour_creator.hh b/src/contour_creator.hh
index 4bc45f5..64c9beb 100644
--- a/src/contour_creator.hh
+++ b/src/contour_creator.hh
@@ -45,6 +45,8 @@ class Point
         int mscase;
         Point * next;
         Point * previous;
+        bool endpoint = false;
+        bool allocated = false;
         Point(int x, int y, int mscase){
             this -> x = x;
             this -> y = y;
diff --git a/src/main.cpp b/src/main.cpp
index 8da17d0..9aa9fae 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -7,12 +7,14 @@
 #include <iostream>
 #include <iterator>
 #include <ostream>
+#include <string>
 #include <vector>
 #include <cstdint>
 #include <gdal/gdal.h>
 #include "gdal/gdal_priv.h"
 #include <gdal/gdal_frmts.h>
 #include <omp.h>
+#include <fstream>
 
 std::vector<Point> produce_cellmap(HeightMap* heightmap, float z)
 {
@@ -52,30 +54,74 @@ std::vector<std::vector<Point>> vector_cellmap(HeightMap* heightmap, int interva
         for (int i = 1; i <= num_contours; i++) 
         {
             auto points = produce_cellmap(heightmap, heightmap->min + interval*i);
-
-                
+            std::vector<Point> line;
+            int x;
+            int y;
+            int points_allocated = 0;
+            x = points[0].x;
+            y = points[0].y;
+            points[0].allocated = true;
+            line.push_back(points[0]);
             for (int j = 0; j < points.size(); j++){
-                Point current = points[j];
-                //std::cout << points_allocated << " " << points.size() << "\n";
                 for (int k = 0; k < points.size(); k++){
-                    Point candidate = points[k];
-                    if (candidate.next != nullptr) {
-                        if (candidate.x +1 == current.x) {
-                            current.next = &candidate;
+                    Point* candidate = &points[k];
+                    if (!candidate->allocated) {
+                        if (candidate->x == x +1 && candidate->y == y) {
+                            candidate->allocated = true;
+                            x = candidate->x;
+                            y = candidate->y;
+                            line.push_back(*candidate);
+                            points_allocated++;
+                            break;
                         }
-                        else if (candidate.x -1 == current.x) {
-                            current.next = &candidate;
+                        else if (candidate->x == x -1 && candidate->y == y) {
+                            x = candidate->x;
+                            y = candidate->y;
+                            candidate->allocated = true;
+                            line.push_back(*candidate);
+                            points_allocated++;
+                            break;
                         }
-                        else if (candidate.y +1 == current.x) {
-                            current.next = &candidate;
+                        else if (candidate->x == x && candidate->y == y + 1) {
+                            x = candidate->x;
+                            y = candidate->y;
+                            candidate->allocated = true;
+                            line.push_back(*candidate);
+                            points_allocated++;
+                            break;
                         }
-                        else if (candidate.y -1 == current.x) {
-                            current.next = &candidate;
+                        else if (candidate->x == x && candidate->y == y - 1) {
+                            x = candidate->x;
+                            y = candidate->y;
+                            candidate->allocated = true;
+                            line.push_back(*candidate);
+                            points_allocated++;
+                            break;
                         }
-                        
                     }
                 }
+                if (j > points_allocated)
+                {
+                    vec_private.push_back(line);
+                    line.clear();
+                    //std::cout << points.size() << " ";
+                    for (int k = 0; k < points.size(); k++){
+                        Point* candidate = &points[k];
+                        if (!candidate->allocated)
+                        {
+                            line.push_back(*candidate);
+                            x = candidate->x;
+                            y = candidate->y;
+                            points_allocated++;
+                            break;
+                        }
+                    }
+                }
+
             }
+            //vec_private.push_back(points);
+            //vector_contours.push_back(points);
+            //std::cout << vector_contours.size();
         }
         #pragma omp critical
 
@@ -140,7 +186,7 @@ void write_output_file(std::vector<std::vector<Point>> all_points, const char *f
 
     OGRLayer *poLayer;
 
-    poLayer = poDS->CreateLayer( "contours", /*&(cellmaps[0]).reference_system*/ NULL, wkbLineString, NULL );
+    poLayer = poDS->CreateLayer( "contours", &heightmap->reference_system, wkbLineString, NULL );
     if( poLayer == NULL )
     {
         printf( "Layer creation failed.\n" );
@@ -162,47 +208,50 @@ void write_output_file(std::vector<std::vector<Point>> all_points, const char *f
 
     for (int j = 0; j < all_points.size(); j++)
     {
-            OGRFeature *feature;
-            OGRLineString *geometry = new OGRLineString();
-            feature = OGRFeature::CreateFeature( poLayer->GetLayerDefn() );
-            feature->SetField( "Name", j );
+        OGRFeature *feature;
+        OGRLineString *geometry = new OGRLineString();
+        feature = OGRFeature::CreateFeature( poLayer->GetLayerDefn() );
+        feature->SetField( "Name", j );
 
 
         std::vector<Point> points = all_points[j];
 
-        for (int i = 0; i < points.size(); i++) 
+        for (int k = 0; k < points.size(); k++)
         {
-
             //int x_raw = i%width;
             //int y_raw = i/height;
-            int x_raw = points[i].x;
-            int y_raw = points[i].y;
+            int x_raw = points[k].x;
+            int y_raw = points[k].y;
+            /*
+            if (x_raw > 500 || y_raw > 400)
+            {
+                std::cout << "🤯";std::cout.flush();
+                continue;
+            }
+            */
             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) );
-            //std::cout << x << ", " << y << "  ";
+            //std::cout << x_raw << ", " << y_raw << "  ";std::cout.flush();
             //geometry->setPoint( geometry->getNumPoints(), x+x_tr*0.25, y+y_tr*0.25 ); 
             //geometry->setPoint( geometry->getNumPoints(), x+x_bl*0.25, y+y_bl*0.25 ); 
             geometry->setPoint(geometry->getNumPoints(),x ,y );
 
-
-                
-            
+            //current sometimes becomes random pointer?????
+          
         }
 
-                    if ( feature->SetGeometry(geometry) != OGRERR_NONE)
-            {
-                printf( "Failed to set geometry.\n" );
-                exit( 1 );
-            }
-            OGRGeometryFactory::destroyGeometry(geometry);
-            if( poLayer->CreateFeature( feature ) != OGRERR_NONE )
-            {
-                printf( "Failed to create feature in shapefile.\n" );
-                exit( 1 );
-            }
-            OGRFeature::DestroyFeature( feature );  
-        
-
+        if ( feature->SetGeometry(geometry) != OGRERR_NONE)
+        {
+            printf( "Failed to set geometry.\n" );
+            exit( 1 );
+        }
+        OGRGeometryFactory::destroyGeometry(geometry);
+        if( poLayer->CreateFeature( feature ) != OGRERR_NONE )
+        {
+            printf( "Failed to create feature in shapefile.\n" );
+            exit( 1 );
+        }
+        OGRFeature::DestroyFeature( feature );
     }
 
     GDALClose( poDS );