#include "heightmap.h"

grbl::heightmap grbl::heightmap::from_params(float from_x, float from_y, float to_x, float to_y, float resolution) {
    heightmap result{};

    result.from_x = from_x;
    result.from_y = from_y;
    result.to_x = to_x;
    result.to_y = to_y;
    result.resolution = resolution;

    result.x_segments = static_cast<size_t>(ceilf((to_x - from_x) / resolution));
    result.y_segments = static_cast<size_t>(ceilf((to_y - from_y) / resolution));

    float y_pos = from_y;
    for (int y = 0; y < (result.y_segments + 1); y++) {
        float x_pos = from_x;
        for (int x = 0; x < (result.x_segments + 1); x++) {
            // add a bit of random wobble to make it visible [-10, +10]
//            float z = ((rand() / (float) RAND_MAX) - 0.5f) * 2.0f * 1.0f;
            float z = 0.0f;
            result.vertices.emplace_back(x_pos, y_pos, z);
            x_pos += resolution;
        }
        y_pos += resolution;
    }

    return result;
}

size_t grbl::heightmap::index_from_coords(float x, float y) const {
    size_t result = 0;
    for (auto& v: vertices) {
        if (v.x == x && v.y == y) {
            return result;
        }
        result++;
    }
    return 0;
}

// bi-linear interpolation
float grbl::heightmap::get_z_at(float x, float y) const {

    // TODO: make faster by precalculating indices

    float x1 = x - fmodf(x, resolution);
    float x2 = x1 + resolution;
    float y1 = y - fmodf(y, resolution);
    float y2 = y1 + resolution;
    float z11 = vertices[index_from_coords(x1, y1)].z;
    float z12 = vertices[index_from_coords(x1, y2)].z;
    float z21 = vertices[index_from_coords(x2, y1)].z;
    float z22 = vertices[index_from_coords(x2, y2)].z;

    float alpha_x = (x - x1) / resolution;
    float a = z11 + (z21 - z11) * alpha_x;
    float b = z12 + (z22 - z12) * alpha_x;

    float alpha_y = (y - y1) / resolution;
    float c = a + (b - a) * alpha_y;

    return c;
}