#include <gdal/cpl_conv.h>
#include <iostream>
#include <stdexcept>
#include <omp.h>
#include <gdal/gdal.h>
#include "gdal/gdal_priv.h"
#include <gdal/gdal_frmts.h>

#include "contour_creator.hh"


HeightMap::HeightMap(const char* filepath) 
{
    // Open the file with gdal:
    const GDALAccess eAccess = GA_ReadOnly;
    GDALDatasetUniquePtr file;
    GDALRegister_GTiff();
    file = GDALDatasetUniquePtr(GDALDataset::FromHandle(GDALOpen( filepath, eAccess )));
    if( !file )
    {
        throw std::runtime_error("Could not open tif file!");
    }
    
    // The heigthmap only has one band
    auto band = file->GetBands()[0];
    
    // write the attrributes
    this->width = band->GetXSize();
    this->height = band->GetYSize();
    this->min = band->GetMinimum();
    this->max = band-> GetMaximum();
    //https://gdal.org/api/gdaldataset_cpp.html#_CPPv4N11GDALDataset15GetGeoTransformEPd
    this->geotransform = (double *) CPLMalloc(sizeof(double)*6);
    this->reference_system = *(file->GetSpatialRef());

    file->GetGeoTransform(this->geotransform);
    
    this->data = (float *) CPLMalloc(sizeof(float)*width*height);
    CPLErr error = band->RasterIO( GF_Read, 0, 0, width, height,
                    this->data, width, height, GDT_Float32,
                    0, 0 );
    if (error) { throw std::runtime_error("Could not read tif file!"); }
    band->FlushCache();
}
float HeightMap::get_pixel(int x, int y)
{
    // all the pixels are in an array of floats from left to right, top to bottom
    int offset = ((this->width * y) + x);
    //std::cout << "  offset: " << offset << " " << x << ","<< y;
    return *(this->data + offset);
}

void HeightMap::blur(float standard_deviation)
{
    // standard_deviation does not do anything yet. This is currently a simple box blur
    int kernel_height = 5;
    int kernel_width = 5;
    int kernel_size = kernel_height*kernel_width;
    float* kernel = (float*) CPLMalloc(sizeof(float)*kernel_size);
    for (int i=0; i<kernel_size; i++)
    {
        *(kernel + i) = 1.0;
    }

    float* blurred = (float *) CPLMalloc(sizeof(float)*this->width*this->height);
    #pragma omp parallel
    {
    #pragma omp for
    for (int i = 0; i < this->height * this->width; i++)
    {
        float blurred_pixel = 0;
        for (int j = 0; j < kernel_size; j++)
        {
            int x = j%kernel_width - kernel_width/2 + i%this->width;
            int y = j/kernel_width - kernel_height/2 + i/this->width;
            if (x<0 || x>=this->width)
            {
                x = 0;
            }
            
            if (y<0 || y>=this->height)
            {
                y = 0;
            }
            blurred_pixel += this->get_pixel(x, y);
        }
        *(blurred + i) = blurred_pixel/float(kernel_size);
    }
    }
    free(this->data);
    this->data = blurred;
    blurred = nullptr;
}