#include <gtest/gtest.h>

#include "heightmap.h"

TEST(heightmap, get_z_at) {
    auto grid = grbl::heightmap::from_params(0, 0, 1, 1, 1);
    grid.vertices[0].z = 0;
    grid.vertices[1].z = 1;
    grid.vertices[2].z = 2;
    grid.vertices[3].z = 3;

    auto z = grid.get_z_at(0.5f, 0.5f);
    EXPECT_EQ(1.5, z);

    z = grid.get_z_at(0.5f, 0);
    EXPECT_EQ(0.5, z);

    z = grid.get_z_at(0.5f, 1);
    EXPECT_EQ(2.5, z);

    z = grid.get_z_at(0, 0.5f);
    EXPECT_EQ(1, z);

    z = grid.get_z_at(1, 0.5f);
    EXPECT_EQ(2, z);
}

TEST(heightmap, get_z_at_non_zero_origin) {
    auto grid = grbl::heightmap::from_params(0 - 0.123, 0 + 0.234, 1 - 0.123, 1 + 0.234, 1);
    grid.vertices[0].z = 0;
    grid.vertices[1].z = 1;
    grid.vertices[2].z = 2;
    grid.vertices[3].z = 3;

    auto z = grid.get_z_at(0.5f - 0.123f, 0.5f + 0.234f);
    EXPECT_EQ(true, grbl::float_equal(1.5, z));

    z = grid.get_z_at(0.5f - 0.123f, 0 + 0.234f);
    EXPECT_EQ(true, grbl::float_equal(0.5, z));

    z = grid.get_z_at(0.5f - 0.123f, 1 + 0.234f);
    EXPECT_EQ(true, grbl::float_equal(2.5, z));

    z = grid.get_z_at(0 - 0.123f, 0.5f + 0.234f);
    EXPECT_EQ(true, grbl::float_equal(1, z));

    z = grid.get_z_at(1 - 0.123f, 0.5f + 0.234f);
    EXPECT_EQ(true, grbl::float_equal(2, z));
}