diff --git a/src/HeightMap.cpp b/src/HeightMap.cpp
index 017a0a4..f3db3d3 100644
--- a/src/HeightMap.cpp
+++ b/src/HeightMap.cpp
@@ -1,3 +1,4 @@
+#include <cstddef>
 #include <gdal/cpl_conv.h>
 #include <iostream>
 #include <stdexcept>
@@ -5,7 +6,7 @@
 #include <gdal/gdal.h>
 #include "gdal/gdal_priv.h"
 #include <gdal/gdal_frmts.h>
-
+//#include "matplotlibcpp.h"
 #include "contour_creator.hh"
 
 
@@ -32,6 +33,7 @@ HeightMap::HeightMap(const char* filepath)
     //https://gdal.org/api/gdaldataset_cpp.html#_CPPv4N11GDALDataset15GetGeoTransformEPd
     this->geotransform = (double *) CPLMalloc(sizeof(double)*6);
     this->reference_system = *(file->GetSpatialRef());
+    this->filepath = filepath;
 
     file->GetGeoTransform(this->geotransform);
     
@@ -41,6 +43,7 @@ HeightMap::HeightMap(const char* filepath)
                     0, 0 );
     if (error) { throw std::runtime_error("Could not read tif file!"); }
     band->FlushCache();
+    const int size = this->width*this->height;
 }
 float HeightMap::get_pixel(int x, int y)
 {
@@ -62,7 +65,7 @@ void HeightMap::blur(float standard_deviation)
         *(kernel + i) = 1.0;
     }
 
-    float* blurred = (float *) CPLMalloc(sizeof(float)*this->width*this->height);
+    float* blurred = new float[this->width*this->height];
     #pragma omp parallel
     {
     #pragma omp for
@@ -90,4 +93,101 @@ void HeightMap::blur(float standard_deviation)
     free(this->data);
     this->data = blurred;
     blurred = nullptr;
+}
+
+void HeightMap::statistics()
+{
+
+    float avg = 0;
+    float var = 0;
+    float std = 0;
+
+    for (int i = 0; i < this->width*this->height; i++)
+        {int x = i%this->width;
+        int y = i/this->width;
+            if (x<0 || x>=this->width)
+            {
+                x = 0;
+            }
+            
+            if (y<0 || y>=this->height)
+            {
+                y = 0;
+            }
+        avg += this->get_pixel(x, y);}
+        //std::cout << this->get_pixel(x, y);}
+    
+    avg = avg/(this->width*this->height);
+    std::cout << "Average value: " << avg <<"\n";
+
+    for (int i = 0; i < this->width*this->height; i++)
+    {
+        int x = i%this->width;
+        int y = i/this->width;
+        var += (avg - this->get_pixel(x, y))*(avg - this->get_pixel(x, y));
+    }
+    std = sqrt(var)/(this->width*this->height-1);
+    var = var / (this->width*this->height-1);
+    std::cout << "std: " << std << "\n";
+    std::cout << "var: " << var << "\n";
+};
+
+
+void HeightMap::calculate_steepness()
+{
+    int kernel_height = 5;
+    int kernel_width = 5;
+    int kernel_size = kernel_height*kernel_width;
+
+    float* kernel = new float[sizeof(float)*kernel_size];
+    float* steepness = new float[this->width*this->height];
+    #pragma omp parallel
+    {
+    #pragma omp for
+    for (int i = 0; i < this->height * this->width; i++)
+    {
+        int x = i%this->width;
+        int y = i/this->width;
+        float max = 0;
+        float min = 0;
+        for (int j = 0; j < kernel_size; j++)
+        {
+            if (this->get_pixel(x, y) > max)
+            {
+                max = get_pixel(x, y);
+            }
+
+            if (this->get_pixel(x, y) < min)
+            {
+                min = get_pixel(x, y);
+            }
+
+            if (x<0 || x>=this->width)
+            {
+                x = 0;
+            }
+            
+            if (y<0 || y>=this->height)
+            {
+                y = 0;
+            }
+            
+        }
+        steepness[i] = max - min;
+    }
+    }
+    GDALAllRegister();
+    GDALDataset *original_file = (GDALDataset *) GDALOpen( filepath, GA_ReadOnly );
+
+    const char * filename = "steepness.tif";
+    auto driver = GetGDALDriverManager()->GetDriverByName("GeoRaster");
+    GDALDataset *file;
+    file = driver->CreateCopy("steepness.tif", original_file, FALSE, NULL, NULL, NULL);
+
+    file->AddBand(GDT_Float32, NULL);
+    GDALRasterBand *band = file->GetRasterBand(0);
+    CPLErr error = band->RasterIO( GF_Write, 0, 0, this->width, this->height,
+                    steepness, this->width, this->height, GDT_Float32,
+                    0, 0 );
+    GDALClose(file);
 }
\ No newline at end of file
diff --git a/src/contour_creator.hh b/src/contour_creator.hh
index 84dc926..841ec0b 100644
--- a/src/contour_creator.hh
+++ b/src/contour_creator.hh
@@ -16,10 +16,12 @@ class HeightMap
         float min; //!< Minimum value in image
         float max; //!< Maximum value in image
         OGRSpatialReference reference_system;
-
         HeightMap(const char* filepath);
+        const char* filepath;
         float get_pixel(int x,int y);
         void blur(float standard_deviation);
+        void statistics();
+        void calculate_steepness();
 };
 
 /**