#include "detector/CollisionDetector.h"
#include "vehicle/ControllableVehicles.h"
#include "config/AirportBounds.h"
#include <gtest/gtest.h>
#include <gmock/gmock.h>
#include "utils/Logger.h"
#include <chrono>
#include <filesystem>

// 在所有测试开始前初始化日志
class GlobalTestEnvironment : public ::testing::Environment {
public:
    void SetUp() override {
        // 确保使用 DEBUG 级别
        Logger::initialize("logs/test.log", LogLevel::DEBUG);
        Logger::debug("=== 测试开始 ===");
    }

    void TearDown() override {
        Logger::debug("=== 测试结束 ===");
        Logger::initialize("", LogLevel::INFO);  // 关闭日志文件
    }
};

int main(int argc, char **argv) {
    ::testing::InitGoogleTest(&argc, argv);
    // 添加全局测试环境
    ::testing::AddGlobalTestEnvironment(new GlobalTestEnvironment);
    return RUN_ALL_TESTS();
}

// Mock ControllableVehicles 类
class MockControllableVehicles : public ControllableVehicles {
public:
    MockControllableVehicles() : ControllableVehicles("") {}  // 使用空字符串，避免加载实际配置
    MOCK_METHOD(bool, isControllable, (const std::string& vehicleNo), (const));
};

// Mock AirportBounds 类
class MockAirportBounds : public AirportBounds {
public:
    MockAirportBounds() : AirportBounds("") {
        // 设置更大的测试边界，以包含所有测试数据
        airportBounds_ = Bounds(0, 0, 4000, 2000);  // 4000x2000 的测试区域
        Logger::info("MockAirportBounds initialized with bounds: ", 
            "x=", airportBounds_.x, ", y=", airportBounds_.y,
            ", width=", airportBounds_.width, ", height=", airportBounds_.height);
    }
    
    // 覆盖原有方法，返回测试用的配置
    AreaType getAreaType(const Vector2D& position) const override { 
        return AreaType::RUNWAY;  // 简化测试，总是返回跑道区域
    }
    
    const AreaConfig& getAreaConfig(AreaType type) const override {
        static const AreaConfig config{50.0, 100.0, 15.0};
        return config;
    }
    
    const Bounds& getAirportBounds() const override {
        return airportBounds_;
    }
};

class CollisionDetectorTest : public ::testing::Test {
protected:
    void SetUp() override {
        // 打印当前工作目录
        std::cout << "Current working directory: " << std::filesystem::current_path() << std::endl;
        
        // 创建 Mock 对象
        airportBounds_ = std::make_unique<MockAirportBounds>();
        mockControllableVehicles_ = std::make_unique<MockControllableVehicles>();
        
        // 创建冲突检测器
        detector_ = std::make_unique<CollisionDetector>(*airportBounds_, *mockControllableVehicles_);
    }

    void TearDown() override {
        Logger::initialize("", LogLevel::INFO);
    }

    std::unique_ptr<MockAirportBounds> airportBounds_;
    std::unique_ptr<MockControllableVehicles> mockControllableVehicles_;
    std::unique_ptr<CollisionDetector> detector_;
};

// 测试可控车辆与航空器的冲突检测
TEST_F(CollisionDetectorTest, DetectControllableVehicleAircraftCollision) {
    // 设置 Mock 期望 - 在创建数据之前设置
    EXPECT_CALL(*mockControllableVehicles_, isControllable("VEH001"))
        .WillRepeatedly(testing::Return(true));
    Logger::info("Set mock expectation: VEH001 is controllable");

    // 设置测试数据
    Aircraft aircraft;
    aircraft.flightNo = "TEST001";
    aircraft.position = {100, 100};
    aircraft.speed = 10;
    aircraft.heading = 90;
    Logger::info("Created aircraft: flightNo=", aircraft.flightNo,
        ", position=(", aircraft.position.x, ", ", aircraft.position.y, ")");

    Vehicle vehicle;
    vehicle.vehicleNo = "VEH001";
    vehicle.position = {120, 100};  // 离航空器20米
    vehicle.speed = 5;
    vehicle.heading = 270;
    Logger::info("Created vehicle: vehicleNo=", vehicle.vehicleNo,
        ", position=(", vehicle.position.x, ", ", vehicle.position.y, ")");

    // 更新交通数据
    detector_->updateTraffic({aircraft}, {vehicle});
    Logger::info("Updated traffic data");

    // 执行冲突检测
    auto risks = detector_->detectCollisions();

    // 验证结果
    ASSERT_EQ(risks.size(), 1);  // 应该检测到一个冲突风险
    if (!risks.empty()) {
        const auto& risk = risks[0];
        EXPECT_EQ(risk.id1, "TEST001");  // 航空器ID
        EXPECT_EQ(risk.id2, "VEH001");   // 车辆ID
        EXPECT_EQ(risk.distance, 20);     // 距离应该是20米
        EXPECT_EQ(risk.level, RiskLevel::CRITICAL);  // 20米距离应该是严重风险
        EXPECT_GT(risk.relativeSpeed, 0); // 相对速度应该大于0
    }
}

// 测试可控车辆与其他车辆的冲突检测
TEST_F(CollisionDetectorTest, DetectControllableVehicleOtherVehicleCollision) {
    // 设置 Mock 期望
    EXPECT_CALL(*mockControllableVehicles_, isControllable("VEH001"))
        .WillRepeatedly(testing::Return(true));
    EXPECT_CALL(*mockControllableVehicles_, isControllable("VEH002"))
        .WillRepeatedly(testing::Return(false));

    // 设置测试数据
    Vehicle controlVehicle;
    controlVehicle.vehicleNo = "VEH001";
    controlVehicle.position = {100, 100};
    controlVehicle.speed = 5;
    controlVehicle.heading = 90;

    Vehicle otherVehicle;
    otherVehicle.vehicleNo = "VEH002";
    otherVehicle.position = {120, 100};  // 距离可控车辆20米
    otherVehicle.speed = 5;
    otherVehicle.heading = 270;

    // 更新交通数据
    detector_->updateTraffic({}, {controlVehicle, otherVehicle});

    // 执行冲突检测
    auto risks = detector_->detectCollisions();

    // 验证结果
    ASSERT_EQ(risks.size(), 1);  // 应该检测到一个冲突风险
    if (!risks.empty()) {
        EXPECT_EQ(risks[0].id1, "VEH001");  // 可控车辆ID
        EXPECT_EQ(risks[0].id2, "VEH002");  // 其他车辆ID
        EXPECT_EQ(risks[0].distance, 20);    // 距离应该是20米
        EXPECT_EQ(risks[0].level, RiskLevel::CRITICAL);  // 20米距离应该是告警
    }
}

// 测试非可控车辆的冲突检测（不应该产生冲突风险）
TEST_F(CollisionDetectorTest, NonControllableVehicleCollision) {
    // 设置测试数据
    Vehicle vehicle1;
    vehicle1.vehicleNo = "VEH001";
    vehicle1.position = {100, 100};
    vehicle1.speed = 5;
    vehicle1.heading = 90;

    Vehicle vehicle2;
    vehicle2.vehicleNo = "VEH002";
    vehicle2.position = {120, 100};  // 距离20米
    vehicle2.speed = 5;
    vehicle2.heading = 270;

    // 设置 Mock 期望
    EXPECT_CALL(*mockControllableVehicles_, isControllable(testing::_))
        .WillRepeatedly(testing::Return(false));

    // 更新交通数据
    detector_->updateTraffic({}, {vehicle1, vehicle2});

    // 执行冲突检测
    auto risks = detector_->detectCollisions();

    // 验证结果
    EXPECT_EQ(risks.size(), 0);  // 非可控车辆之间的冲突不应该被检测
}

// 测试多个可控车辆之间的冲突检测
TEST_F(CollisionDetectorTest, MultipleControllableVehiclesCollision) {
    // 设置 Mock 期望 - 所有车辆都是可控的
    ON_CALL(*mockControllableVehicles_, isControllable(testing::_))
        .WillByDefault(testing::Return(true));
    EXPECT_CALL(*mockControllableVehicles_, isControllable(testing::_))
        .Times(testing::AtLeast(3));  // 至少调用3次，因为有3辆车
    Logger::info("Set mock expectation: all vehicles are controllable");

    // 设置测试数据
    std::vector<Vehicle> vehicles;
    
    // VEH001 在 (100, 100)
    Vehicle v1;
    v1.vehicleNo = "VEH001";
    v1.position = {100.0, 100.0};
    v1.speed = 5;
    v1.heading = 90;
    vehicles.push_back(v1);
    
    // VEH002 在 (120, 100)，与 VEH001 相距 20 米
    Vehicle v2;
    v2.vehicleNo = "VEH002";
    v2.position = {120.0, 100.0};
    v2.speed = 5;
    v2.heading = 90;
    vehicles.push_back(v2);
    
    // VEH003 在 (200, 100)，与其他车辆距离超过阈值
    Vehicle v3;
    v3.vehicleNo = "VEH003";
    v3.position = {200.0, 100.0};
    v3.speed = 5;
    v3.heading = 90;
    vehicles.push_back(v3);

    // 更新交通数据
    detector_->updateTraffic({}, vehicles);

    // 执行冲突检测
    auto risks = detector_->detectCollisions();

    // 验证结果
    EXPECT_EQ(risks.size(), 1);  // 应该只检测到1个冲突风险（VEH001和VEH002之间）
    
    if (risks.size() == 1) {
        // 验证冲突风险的详细信息
        EXPECT_TRUE((risks[0].id1 == "VEH001" && risks[0].id2 == "VEH002") ||
                   (risks[0].id1 == "VEH002" && risks[0].id2 == "VEH001"));
        EXPECT_EQ(risks[0].distance, 20.0);
        EXPECT_EQ(risks[0].level, RiskLevel::CRITICAL);  // 20米应该是告警级别
    }
}

// 性能测试：模拟真实机场场景
TEST_F(CollisionDetectorTest, PerformanceTest) {
    // 设置 Mock 期望 - 默认车辆不可控
    EXPECT_CALL(*mockControllableVehicles_, isControllable(testing::_))
        .WillRepeatedly(testing::Return(false));
    
    // 设置3辆可控车辆
    std::vector<std::string> controlVehicleNos = {
        "VEH001",  // 滑行道上的可控车辆
        "VEH002",  // 停机位的可控车辆
        "VEH003"   // 服务区的可控车辆
    };
    
    for (const auto& vehicleNo : controlVehicleNos) {
        EXPECT_CALL(*mockControllableVehicles_, isControllable(vehicleNo))
            .WillRepeatedly(testing::Return(true));
    }
    Logger::info("Set mock expectations for controllable vehicles");

    // 创建测试数据
    std::vector<Aircraft> aircraft;
    std::vector<Vehicle> vehicles;
    
    // 跑道区域：5架航空器
    for (int i = 0; i < 5; ++i) {
        Aircraft a;
        a.flightNo = "RW" + std::to_string(i + 1);
        a.position = {1800.0 + i * 500, 30.0};  // 跑道上等间距分布
        a.speed = 30;  // 较快速度
        a.heading = 90;
        aircraft.push_back(a);
    }
    
    // 滑行道：5架航空器
    for (int i = 0; i < 5; ++i) {
        Aircraft a;
        a.flightNo = "TW" + std::to_string(i + 1);
        a.position = {1800.0 + i * 500, 90.0};  // 滑行道上等间距分布
        a.speed = 10;  // 中等速度
        a.heading = 90;
        aircraft.push_back(a);
    }
    
    // 停机位区域：100架航空器，180辆车辆
    for (int i = 0; i < 100; ++i) {
        Aircraft a;
        a.flightNo = "GT" + std::to_string(i + 1);
        a.position = {
            750.0 + (i % 10) * 150,  // 10列
            500.0 + (i / 10) * 100   // 10行
        };
        a.speed = 0;  // 静止
        a.heading = 180;
        aircraft.push_back(a);
    }

    for (int i = 0; i < 180; ++i) {
        Vehicle v;
        v.vehicleNo = "GV" + std::to_string(i + 1);
        v.position = {
            750.0 + (i % 12) * 125,  // 12列
            500.0 + (i / 12) * 83    // 15行
        };
        v.speed = 5;  // 低速
        v.heading = (i % 4) * 90;  // 4个方向
        vehicles.push_back(v);
    }
    
    // 服务区：40架航空器，120辆车辆
    for (int i = 0; i < 40; ++i) {
        Aircraft a;
        a.flightNo = "SA" + std::to_string(i + 1);
        a.position = {
            1000.0 + (i % 8) * 250,  // 8列
            500.0 + (i / 8) * 200    // 5行
        };
        a.speed = 0;  // 静止
        a.heading = 180;
        aircraft.push_back(a);
    }
    
    for (int i = 0; i < 120; ++i) {
        Vehicle v;
        v.vehicleNo = "SV" + std::to_string(i + 1);
        v.position = {
            1000.0 + (i % 10) * 200,  // 10列
            500.0 + (i / 10) * 100    // 12行
        };
        v.speed = 8;  // 中等速度
        v.heading = (i % 8) * 45;  // 8个方向
        vehicles.push_back(v);
    }

    // 添加3辆可控车辆
    // 1. 滑行道上的可控车辆
    Vehicle taxiwayVehicle;
    taxiwayVehicle.vehicleNo = "VEH001";
    taxiwayVehicle.position = {1800.0, 90.0};  // 在滑行道上
    taxiwayVehicle.speed = 10;
    taxiwayVehicle.heading = 90;
    vehicles.push_back(taxiwayVehicle);
    
    // 2. 停机位的可控车辆
    Vehicle gateVehicle;
    gateVehicle.vehicleNo = "VEH002";
    gateVehicle.position = {750.0, 500.0};  // 在停机位区域
    gateVehicle.speed = 5;
    gateVehicle.heading = 180;
    vehicles.push_back(gateVehicle);
    
    // 3. 服务区的可控车辆
    Vehicle serviceVehicle;
    serviceVehicle.vehicleNo = "VEH003";
    serviceVehicle.position = {1000.0, 500.0};  // 在服务区
    serviceVehicle.speed = 8;
    serviceVehicle.heading = 270;
    vehicles.push_back(serviceVehicle);

    // 更新交通数据
    detector_->updateTraffic(aircraft, vehicles);
    Logger::info("Updated traffic data with ", aircraft.size(), " aircraft and ", 
            vehicles.size(), " vehicles (including 3 controllable vehicles)");

    // 执行冲突检测并记录时间
    auto start = std::chrono::high_resolution_clock::now();
    
    auto risks = detector_->detectCollisions();
    
    auto end = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
    Logger::info("Collision detection completed in ", duration.count(), " microseconds");
    Logger::info("Found ", risks.size(), " risks");

    // 验证结果
    ASSERT_GT(risks.size(), 0);  // 应该检测到一些冲突风险
    
    // 验证性能要求
    EXPECT_LT(duration.count(), 100000);  // 期望处理时间小于100ms
}

// 修改矩形碰撞检测测试
TEST_F(CollisionDetectorTest, RectangleCollisionParallelMotion) {
    // 设置系统配置
    SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0;
    
    // 创建两个平行运动的车辆
    Vehicle v1;
    v1.vehicleNo = "VEH001";
    v1.position = {100.0, 100.0};
    v1.speed = 10.0;
    v1.heading = 90.0;  // 向东行驶
    
    Vehicle v2;
    v2.vehicleNo = "VEH002";
    v2.position = {100.0, 107.0};  // 在v1北侧7米处
    v2.speed = 10.0;
    v2.heading = 90.0;  // 向东行驶
    
    // 检测碰撞
    bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
    
    // 由于两车间距小于安全距离，应该检测到碰撞
    EXPECT_TRUE(hasCollision) << "平行运动的车辆应该检测到碰撞";
}

TEST_F(CollisionDetectorTest, RectangleCollisionCrossingPaths) {
    // 设置系统配置
    SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0;
    
    // 创建两个交叉路径的车辆
    Vehicle v1;
    v1.vehicleNo = "VEH001";
    v1.position = {0.0, 100.0};
    v1.speed = 10.0;
    v1.heading = 90.0;  // 向东行驶
    
    Vehicle v2;
    v2.vehicleNo = "VEH002";
    v2.position = {100.0, 200.0};
    v2.speed = 10.0;
    v2.heading = 180.0;  // 向南行驶
    
    // 检测碰撞
    bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
    
    // 两车将在(100, 100)处相遇，应该检测到碰撞
    EXPECT_TRUE(hasCollision) << "交叉路径的车辆应该检测到碰撞";
}

TEST_F(CollisionDetectorTest, RectangleCollisionAircraftVehicle) {
    // 设置系统配置
    SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0;
    SystemConfig::instance().collision_detection.prediction.aircraft_size = 10.0;
    
    // 创建一个航空器和一个车辆
    Aircraft aircraft;
    aircraft.flightNo = "TEST001";
    aircraft.position = {100.0, 100.0};
    aircraft.speed = 15.0;
    aircraft.heading = 90.0;  // 向东行驶
    
    Vehicle vehicle;
    vehicle.vehicleNo = "VEH001";
    vehicle.position = {120.0, 105.0};  // 在航空器前方偏北5米处
    vehicle.speed = 8.0;
    vehicle.heading = 270.0;  // 向西行驶（与航空器相向而行）
    
    std::cout << "\n=== 矩形碰撞检测测试（航空器-车辆） ===" << std::endl;
    std::cout << "配置信息：" << std::endl;
    std::cout << "- 航空器尺寸: " << SystemConfig::instance().collision_detection.prediction.aircraft_size << "米" << std::endl;
    std::cout << "- 车辆尺寸: " << SystemConfig::instance().collision_detection.prediction.vehicle_size << "米" << std::endl;
    
    std::cout << "\n航空器信息：" << std::endl;
    std::cout << "- 编号: " << aircraft.flightNo << std::endl;
    std::cout << "- 位置: (" << aircraft.position.x << ", " << aircraft.position.y << ")" << std::endl;
    std::cout << "- 速度: " << aircraft.speed << "米/秒" << std::endl;
    std::cout << "- 朝向: " << aircraft.heading << "度" << std::endl;
    
    std::cout << "\n车辆信息：" << std::endl;
    std::cout << "- 编号: " << vehicle.vehicleNo << std::endl;
    std::cout << "- 位置: (" << vehicle.position.x << ", " << vehicle.position.y << ")" << std::endl;
    std::cout << "- 速度: " << vehicle.speed << "米/秒" << std::endl;
    std::cout << "- 朝向: " << vehicle.heading << "度" << std::endl;
    
    // 计算实际距离
    double dx = vehicle.position.x - aircraft.position.x;
    double dy = vehicle.position.y - aircraft.position.y;
    double distance = std::sqrt(dx*dx + dy*dy);
    std::cout << "\n实际距离: " << distance << "米" << std::endl;
    
    // 计算相对速度
    double relativeSpeed = aircraft.speed + vehicle.speed;  // 相向而行，速度相加
    std::cout << "相对速度: " << relativeSpeed << "米/秒" << std::endl;
    
    // 预计碰撞时间
    double timeToCollision = distance / relativeSpeed;
    std::cout << "预计碰撞时间: " << timeToCollision << "秒" << std::endl;
    
    // 检测碰撞
    bool hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle);
    std::cout << "\n碰撞检测结果: " << (hasCollision ? "是" : "否") << std::endl;
    
    // 由于距离较近且相向而行，应该检测到碰撞
    EXPECT_TRUE(hasCollision) << "相向而行且距离较近的航空器和车辆应该检测到碰撞";
    
    // 测试边界情况：增加距离
    vehicle.position.x = 150.0;  // 增加到50米距离
    hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle);
    EXPECT_FALSE(hasCollision) << "距离较远时不应该检测到碰撞";
    
    // 测试边界情况：静止状态
    vehicle.position.x = 110.0;  // 距离缩短到10米
    vehicle.speed = 0.0;
    aircraft.speed = 0.0;
    hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle);
    EXPECT_TRUE(hasCollision) << "静止状态下距离较近时应该检测到碰撞";
    
    // 测试边界情况：垂直路径
    vehicle.position = {100.0, 120.0};  // 在航空器正北方
    vehicle.speed = 10.0;
    vehicle.heading = 180.0;  // 向南行驶
    aircraft.speed = 10.0;
    aircraft.heading = 90.0;  // 向东行驶
    hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle);
    EXPECT_TRUE(hasCollision) << "垂直路径且距离较近时应该检测到碰撞";
}

TEST_F(CollisionDetectorTest, RectangleCollisionWithRotation) {
    // 设置系统配置
    SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0;
    
    // 创建两个有一定角度的车辆
    Vehicle v1;
    v1.vehicleNo = "VEH001";
    v1.position = {100.0, 100.0};
    v1.speed = 10.0;
    v1.heading = 45.0;  // 向东北行驶
    
    Vehicle v2;
    v2.vehicleNo = "VEH002";
    v2.position = {105.0, 105.0};  // 距离缩短到约7.07米
    v2.speed = 10.0;
    v2.heading = 315.0;  // 向西北行驶
    
    // 检测碰撞
    bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
    
    // 由于两车轨迹交叉且距离较近，应该检测到碰撞
    EXPECT_TRUE(hasCollision) << "不同角度的车辆相交时应该检测到碰撞";
    
    // 将v2移动到更远的位置
    v2.position = {150.0, 150.0};
    hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
    
    // 距离较远时不应该检测到碰撞
    EXPECT_FALSE(hasCollision) << "距离较远时不应该检测到碰撞";
}

TEST_F(CollisionDetectorTest, RectangleCollisionEdgeCases) {
    // 设置系统配置
    SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0;
    
    Vehicle v1;
    v1.vehicleNo = "VEH001";
    v1.position = {100.0, 100.0};
    v1.speed = 0.0;  // 静止
    v1.heading = 90.0;
    
    Vehicle v2;
    v2.vehicleNo = "VEH002";
    
    // 测试1：两个静止车辆，刚好接触
    v2.position = {110.0, 100.0};  // 距离为10米（两车长度和）
    v2.speed = 0.0;
    v2.heading = 90.0;
    
    bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
    EXPECT_TRUE(hasCollision) << "两个静止且刚好接触的车辆应该检测到碰撞";
    
    // 测试2：两个静止车辆，距离略大于安全距离
    v2.position = {111.0, 100.0};  // 距离为11米
    hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
    EXPECT_FALSE(hasCollision) << "两个静止且距离大于安全距离的车辆不应该检测到碰撞";
    
    // 测试3：极低速度
    v2.position = {120.0, 100.0};
    v2.speed = 0.1;  // 极低速度
    hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
    EXPECT_FALSE(hasCollision) << "距离足够且速度极低的车辆不应该检测到碰撞";
}