#include <cstddef>
#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 = new double[6];
    this->reference_system = *(file->GetSpatialRef());
    this->filepath = filepath;

    file->GetGeoTransform(this->geotransform);
    
    this->data = new 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();
    const int size = this->width*this->height;
}
float HeightMap::get_pixel(int x, int y)
{
    // all the pixels are in an array of floats from left to right, top to bottom
    return this->data[((this->width * y) + x)];
}

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 = new float [kernel_size];
    for (int i=0; i<kernel_size; i++)
    {
        kernel[i] = 1.0;
    }

    float* blurred = new 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);
    }
    }

    delete[] kernel;
    delete[] 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(const char* filepath)
{
    int kernel_height = 5;
    int kernel_width = 5;
    int kernel_size = kernel_height*kernel_width;

    float* kernel = new 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( this->filepath, GA_ReadOnly );

    auto driver = GetGDALDriverManager()->GetDriverByName("GeoRaster");
    GDALDataset *file;
    file = driver->CreateCopy(filepath, 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);
}