#include <gtest/gtest.h>

#include "controller/PlaneZRegistry.hpp"

using geopro::controller::PlaneZRegistry;

TEST(PlaneZRegistry, FirstCheckSetsPlaneZ) {
    PlaneZRegistry r;
    EXPECT_DOUBLE_EQ(r.onChecked("trajectory", "a", 12.0), 12.0);
    EXPECT_TRUE(r.hasPlane("trajectory"));
    EXPECT_DOUBLE_EQ(r.planeZ("trajectory"), 12.0);
}
TEST(PlaneZRegistry, SecondCheckKeepsFirstZ) {
    PlaneZRegistry r;
    r.onChecked("trajectory", "a", 12.0);
    EXPECT_DOUBLE_EQ(r.onChecked("trajectory", "b", 99.0), 12.0);  // 投影到首个 ds 的平面
}
TEST(PlaneZRegistry, PlaneDisappearsWhenAllUnchecked) {
    PlaneZRegistry r;
    r.onChecked("trajectory", "a", 12.0);
    r.onChecked("trajectory", "b", 99.0);
    r.onUnchecked("trajectory", "a");
    EXPECT_TRUE(r.hasPlane("trajectory"));   // 还有 b
    r.onUnchecked("trajectory", "b");
    EXPECT_FALSE(r.hasPlane("trajectory"));  // 全消 → 平面消失
}
TEST(PlaneZRegistry, RecheckAfterEmptyResetsZ) {
    PlaneZRegistry r;
    r.onChecked("trajectory", "a", 12.0);
    r.onUnchecked("trajectory", "a");
    EXPECT_DOUBLE_EQ(r.onChecked("trajectory", "c", 7.0), 7.0);  // 重新首勾 → 新 z
}
TEST(PlaneZRegistry, SetPlaneZMovesPlane) {
    PlaneZRegistry r;
    r.onChecked("trajectory", "a", 12.0);
    r.setPlaneZ("trajectory", 30.0);
    EXPECT_DOUBLE_EQ(r.planeZ("trajectory"), 30.0);
}