#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 (float_equal(v.x, x) && float_equal(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

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

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

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

    return (float) c;
}

bool grbl::float_equal(float a, float b) {
    return fabs(a - b) < 0.0001;
}