CollisionAvoidance/tests/SafetyZoneTest.cpp

#include <gtest/gtest.h>
#include "detector/SafetyZone.h"
#include "utils/Logger.h"
#include "config/SystemConfig.h"

class SafetyZoneTest : public ::testing::Test {
protected:
    void SetUp() override {
        // 确保 SystemConfig 已初始化
        SystemConfig::instance();
        // 创建一个安全区，中心点在(0,0)，飞机安全区半径50米，特勤车安全区半径30米
        safetyZone = std::make_unique<SafetyZone>(Vector2D{0, 0}, 50.0, 30.0);
    }
    
    std::unique_ptr<SafetyZone> safetyZone;
    
    // 创建测试用的移动对象
    Aircraft createAircraft(const std::string& id, double x, double y) {
        Aircraft obj;
        obj.id = id;
        obj.position = Vector2D{x, y};
        obj.geo.latitude = 0;  // 这里的经纬度不重要
        obj.geo.longitude = 0;
        obj.type = MovingObjectType::AIRCRAFT;  // 设置类型为航空器
        return obj;
    }
    
    Vehicle createSpecialVehicle(const std::string& id, double x, double y) {
        Vehicle obj;
        obj.id = id;
        obj.position = Vector2D{x, y};
        obj.geo.latitude = 0;
        obj.geo.longitude = 0;
        obj.type = MovingObjectType::SPECIAL;  // 设置类型为特勤车
        obj.isControllable = false;  // 设置为非可控车辆（即特勤车）
        return obj;
    }
    
    Vehicle createUnmannedVehicle(const std::string& id, double x, double y) {
        Vehicle obj;
        obj.id = id;
        obj.position = Vector2D{x, y};
        obj.geo.latitude = 0;
        obj.geo.longitude = 0;
        obj.type = MovingObjectType::UNMANNED;  // 设置类型为无人车
        obj.isControllable = true;  // 设置为可控车辆（即无人车）
        return obj;
    }
    
    // 获取飞机有效距离（安全区半径 + 飞机半长度）
    double getAircraftEffectiveDistance() const {
        const auto& config = SystemConfig::instance().collision_detection.prediction;
        return safetyZone->getAircraftRadius() + config.aircraft_size / 2.0;
    }
    
    // 获取特勤车有效距离（安全区半径 + 车辆半长度）
    double getVehicleEffectiveDistance() const {
        const auto& config = SystemConfig::instance().collision_detection.prediction;
        return safetyZone->getSpecialVehicleRadius() + config.vehicle_size / 2.0;
    }
};

// 测试初始状态
TEST_F(SafetyZoneTest, InitialState) {
    EXPECT_EQ(safetyZone->getState(), SafetyZoneState::INACTIVE);
    EXPECT_EQ(safetyZone->getType(), SafetyZoneType::NONE);
    EXPECT_EQ(safetyZone->getCurrentRadius(), 0.0);
}

// 测试飞机进入安全区
TEST_F(SafetyZoneTest, AircraftEntering) {
    double effectiveDistance = getAircraftEffectiveDistance();
    
    // 创建一个飞机，位置在安全区边缘内侧
    auto aircraft = createAircraft("AC001", effectiveDistance - 1.0, 0.0);
    
    // 检查飞机是否在安全区内
    EXPECT_TRUE(safetyZone->isObjectInZone(aircraft));
    EXPECT_EQ(safetyZone->getType(), SafetyZoneType::AIRCRAFT);
    EXPECT_EQ(safetyZone->getCurrentRadius(), 50.0);
    
    // 测试飞机完全离开安全区
    aircraft = createAircraft("AC001", effectiveDistance + 1.0, 0.0);
    EXPECT_FALSE(safetyZone->isObjectInZone(aircraft));
}

// 测试特勤车进入安全区
TEST_F(SafetyZoneTest, SpecialVehicleEntering) {
    double effectiveDistance = getVehicleEffectiveDistance();
    
    // 创建一个特勤车，位置在安全区边缘内侧
    auto vehicle = createSpecialVehicle("TQ001", effectiveDistance - 1.0, 0.0);
    
    // 检查特勤车是否在安全区内
    EXPECT_TRUE(safetyZone->isObjectInZone(vehicle));
    EXPECT_EQ(safetyZone->getType(), SafetyZoneType::VEHICLE);
    EXPECT_EQ(safetyZone->getCurrentRadius(), 30.0);
    
    // 测试特勤车完全离开安全区
    vehicle = createSpecialVehicle("TQ001", effectiveDistance + 1.0, 0.0);
    EXPECT_FALSE(safetyZone->isObjectInZone(vehicle));
}

// 测试无人车进入安全区
TEST_F(SafetyZoneTest, UnmannedVehicleEntering) {
    // 创建一个无人车，位置在安全区内
    auto vehicle = createUnmannedVehicle("UV001", 10.0, 0.0);
    
    // 检查无人车是否在安全区内（应该返回false，因为无人车不能设置安全区类型）
    EXPECT_FALSE(safetyZone->isObjectInZone(vehicle));
    EXPECT_EQ(safetyZone->getType(), SafetyZoneType::NONE);
    EXPECT_EQ(safetyZone->getCurrentRadius(), 0.0);
}

// 测试安全区类型锁定
TEST_F(SafetyZoneTest, SafetyZoneTypeLocking) {
    double aircraftEffectiveDistance = getAircraftEffectiveDistance();
    double vehicleEffectiveDistance = getVehicleEffectiveDistance();
    
    // 先让飞机进入安全区
    auto aircraft = createAircraft("AC001", aircraftEffectiveDistance - 5.0, 0.0);
    EXPECT_TRUE(safetyZone->isObjectInZone(aircraft));
    EXPECT_EQ(safetyZone->getType(), SafetyZoneType::AIRCRAFT);
    
    // 尝试让特勤车进入已经被飞机设置类型的安全区
    auto vehicle = createSpecialVehicle("TQ001", vehicleEffectiveDistance - 5.0, 0.0);
    EXPECT_FALSE(safetyZone->isObjectInZone(vehicle));
    
    // 确认安全区类型和尺寸没有改变
    EXPECT_EQ(safetyZone->getType(), SafetyZoneType::AIRCRAFT);
    EXPECT_EQ(safetyZone->getCurrentRadius(), 50.0);
}

// 测试边界条件
TEST_F(SafetyZoneTest, BoundaryConditions) {
    double aircraftEffectiveDistance = getAircraftEffectiveDistance();
    double vehicleEffectiveDistance = getVehicleEffectiveDistance();
    
    // 测试飞机在边界上
    auto aircraft1 = createAircraft("AC001", aircraftEffectiveDistance, 0.0);
    EXPECT_TRUE(safetyZone->isObjectInZone(aircraft1));
    
    // 测试飞机在边界外
    auto aircraft2 = createAircraft("AC002", aircraftEffectiveDistance + 0.1, 0.0);
    EXPECT_FALSE(safetyZone->isObjectInZone(aircraft2));
    
    // 测试特勤车在边界上
    safetyZone = std::make_unique<SafetyZone>(Vector2D{0, 0}, 50.0, 30.0);
    auto vehicle1 = createSpecialVehicle("TQ001", vehicleEffectiveDistance, 0.0);
    EXPECT_TRUE(safetyZone->isObjectInZone(vehicle1));
    
    // 测试特勤车在边界外
    auto vehicle2 = createSpecialVehicle("TQ002", vehicleEffectiveDistance + 0.1, 0.0);
    EXPECT_FALSE(safetyZone->isObjectInZone(vehicle2));
}

// 测试状态重置
TEST_F(SafetyZoneTest, StateReset) {
    double effectiveDistance = getAircraftEffectiveDistance();
    
    // 先让飞机进入安全区
    auto aircraft = createAircraft("AC001", effectiveDistance - 5.0, 0.0);
    EXPECT_TRUE(safetyZone->isObjectInZone(aircraft));
    
    // 设置状态为激活
    safetyZone->setState(SafetyZoneState::ACTIVE);
    EXPECT_EQ(safetyZone->getState(), SafetyZoneState::ACTIVE);
    
    // 重置状态
    safetyZone->setState(SafetyZoneState::INACTIVE);
    EXPECT_EQ(safetyZone->getState(), SafetyZoneState::INACTIVE);
    
    // 确认类型和尺寸保持不变
    EXPECT_EQ(safetyZone->getType(), SafetyZoneType::AIRCRAFT);
    EXPECT_EQ(safetyZone->getCurrentRadius(), 50.0);
}