#include <gtest/gtest.h>
#include "geo/LocalFrame.hpp"
using geopro::core::LocalFrame;

TEST(LocalFrame, GisToWorldSubtractsOriginEastNorth) {
    LocalFrame f(/*originEast=*/516000.0, /*originNorth=*/2494000.0, /*zDatum=*/0.0);
    auto w = f.gisToWorld(516863.6350992983, 2494259.56246985, 21.0);
    EXPECT_NEAR(w.x, 863.6350992983, 1e-6);
    EXPECT_NEAR(w.y, 259.56246985, 1e-6);
    EXPECT_NEAR(w.z, 21.0, 1e-6);
}

TEST(LocalFrame, RoundTripWorldGis) {
    LocalFrame f(516000.0, 2494000.0, 5.0);
    auto w = f.gisToWorld(516500.0, 2494500.0, 30.0);
    auto g = f.worldToGis(w);
    EXPECT_NEAR(g.east, 516500.0, 1e-6);
    EXPECT_NEAR(g.north, 2494500.0, 1e-6);
    EXPECT_NEAR(g.elevation, 30.0, 1e-6);
}