#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>

// Mock ControllableVehicles 类
class MockControllableVehicles {
public:
    MockControllableVehicles() = default;

    MOCK_METHOD(bool, isControllable, (const std::string& vehicleNo), (const));
    MOCK_METHOD(const ControllableVehicleConfig*, findVehicle, (const std::string& vehicleNo), (const));
    MOCK_METHOD(void, sendCommand, (const std::string& vehicleNo, const VehicleCommand& cmd));

    // 转换为 ControllableVehicles& 的操作符
    operator const ControllableVehicles&() const {
        return ControllableVehicles::getInstance();
    }
};

// Mock AirportBounds 类
class MockAirportBounds : public AirportBounds {
public:
    MockAirportBounds() : AirportBounds("") {
        // 设置测试区域边界
        airportBounds_ = Bounds(0, 0, 5000, 4000);
        areaBounds_[AreaType::TEST_ZONE] = airportBounds_;
        
        // 设置测试区域配置
        AreaConfig config;
        config.collision_radius = {50.0, 50.0, 25.0};  // aircraft, special, unmanned
        config.height_threshold = 10.0;
        config.warning_zone_radius = {100.0, 100.0, 50.0};
        config.alert_zone_radius = {50.0, 50.0, 25.0};
        areaConfigs_[AreaType::TEST_ZONE] = config;
    }
    
    MOCK_CONST_METHOD1(getAreaType, AreaType(const Vector2D& position));
    
    const AreaConfig& getAreaConfig(AreaType type) const override {
        auto it = areaConfigs_.find(type);
        if (it == areaConfigs_.end()) {
            throw std::runtime_error("Invalid area type");
        }
        return it->second;
    }
    
    const Bounds& getAirportBounds() const override {
        return airportBounds_;
    }
};

class CollisionDetectorTest : public ::testing::Test {
protected:
    void SetUp() override {
        // 设置日志级别为 DEBUG
        Logger::initialize("", LogLevel::DEBUG);
        
        // 打印当前工作目录
        std::cout << "Current working directory: " << std::filesystem::current_path() << std::endl;
        
        // 创建 Mock 对象
        airportBounds_ = std::make_unique<MockAirportBounds>();
        mockControllableVehicles_ = std::make_unique<MockControllableVehicles>();
        
        // 设置 Mock 对象的行为
        EXPECT_CALL(*airportBounds_, getAreaType(::testing::_))
            .WillRepeatedly(::testing::Return(AreaType::TEST_ZONE));
        
        // 创建冲突检测器
        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, UnmannedVehicleAircraftCollision) {
    // 设置测试数据
    Aircraft aircraft;
    aircraft.flightNo = "TEST001";
    aircraft.position = {100.0, 100.0};
    aircraft.speed = 15.0;
    aircraft.heading = 90.0;  // 向东行驶
    aircraft.type = MovingObjectType::AIRCRAFT;
    
    Vehicle vehicle;
    vehicle.vehicleNo = "VEH001";
    vehicle.position = {120.0, 105.0};  // 在航空器前方偏北5米处
    vehicle.speed = 8.0;
    vehicle.heading = 270.0;  // 向西行驶（与航空器相向而行）
    vehicle.type = MovingObjectType::UNMANNED;
    vehicle.isControllable = true;
    
    // 测试1：相向而行且距离较近
    auto collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
    EXPECT_TRUE(collisionResult.willCollide) << "相向而行且距离较近的航空器和无人车应该检测到碰撞";
    
    // 测试2：增加距离到边界值
    vehicle.position = {150.0, 105.0};  // 增加到50米距离
    collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
    EXPECT_TRUE(collisionResult.willCollide) << "在警告距离内应该检测到碰撞";
    
    // 测试3：超出碰撞检测范围
    vehicle.position = {900.0, 100.0};  // 增加到800米距离
    collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
    EXPECT_FALSE(collisionResult.willCollide) << "距离较远时不应该检测到碰撞";
}

// 测试无人车与特勤车的碰撞检测
TEST_F(CollisionDetectorTest, UnmannedVehicleSpecialVehicleCollision) {
    Vehicle unmanned;
    unmanned.vehicleNo = "VEH001";
    unmanned.position = {100.0, 100.0};
    unmanned.speed = 10.0;
    unmanned.heading = 90.0;  // 向东行驶
    unmanned.type = MovingObjectType::UNMANNED;
    unmanned.isControllable = true;
    
    Vehicle special;
    special.vehicleNo = "VEH002";
    special.position = {120.0, 100.0};  // 距离20米
    special.speed = 5.0;
    special.heading = 270.0;  // 向西行驶
    special.type = MovingObjectType::SPECIAL;
    special.isControllable = false;
    
    // 测试1：相向而行且距离较近
    auto collisionResult = detector_->checkCollision(unmanned, special, 30.0);
    EXPECT_TRUE(collisionResult.willCollide) << "相向而行且距离较近的无人车和特勤车应该检测到碰撞";
    
    // 测试2：在警告距离边界
    special.position = {150.0, 100.0};  // 增加到50米距离
    collisionResult = detector_->checkCollision(unmanned, special, 30.0);
    EXPECT_TRUE(collisionResult.willCollide) << "在警告距离应该检测到碰撞";
    
    // 测试3：超出碰撞检测范围
    special.position = {300.0, 100.0};  // 增加到200米距离
    collisionResult = detector_->checkCollision(unmanned, special, 30.0);
    EXPECT_FALSE(collisionResult.willCollide) << "超出警告距离时不应该检测到碰撞";
}

// 测试斜向叉路径碰撞
TEST_F(CollisionDetectorTest, DiagonalCrossingPathsCollision) {
    Vehicle v1;
    v1.vehicleNo = "VEH001";
    v1.position = {100.0, 100.0};
    v1.speed = 10.0;
    v1.heading = 45.0;  // 北行驶
    v1.type = MovingObjectType::UNMANNED;
    v1.isControllable = true;
    
    Vehicle v2;
    v2.vehicleNo = "VEH002";
    v2.position = {150.0, 150.0};
    v2.speed = 10.0;
    v2.heading = 225.0;  // 向西南行驶（与v1相向）
    v2.type = MovingObjectType::UNMANNED;
    v2.isControllable = true;
    
    // 测试1：路径交叉且距离较近
    auto collisionResult = detector_->checkCollision(v1, v2, 30.0);
    EXPECT_TRUE(collisionResult.willCollide) << "斜向相向而行且路径交叉的车辆应该检测到碰撞";
    
    // 测试2：不同速度比
    v1.speed = 5.0;   // 降低速度
    v2.speed = 15.0;  // 提高速度
    collisionResult = detector_->checkCollision(v1, v2, 30.0);
    EXPECT_TRUE(collisionResult.willCollide) << "不同速度比的斜向相向车辆应该检测到碰撞";
    
    // 测试3：超出碰撞检测范围
    v2.position = {300.0, 300.0};  // 增加到较远距离
    collisionResult = detector_->checkCollision(v1, v2, 30.0);
    EXPECT_FALSE(collisionResult.willCollide) << "距离较远时不应该检测到碰撞";
}

// 测试垂直交叉路径碰撞
TEST_F(CollisionDetectorTest, PerpendicularCrossingPathsCollision) {
    Vehicle v1;
    v1.vehicleNo = "VEH001";
    v1.position = {100.0, 100.0};
    v1.speed = 10.0;
    v1.heading = 90.0;  // 向东行驶
    v1.type = MovingObjectType::UNMANNED;
    v1.isControllable = true;
    
    Vehicle v2;
    v2.vehicleNo = "VEH002";
    v2.position = {120.0, 120.0};  // 在v1东北方20米处
    v2.speed = 10.0;
    v2.heading = 180.0;  // 向南行驶
    v2.type = MovingObjectType::UNMANNED;
    v2.isControllable = true;
    
    // 测试1：垂直相交且距离较近
    auto collisionResult = detector_->checkCollision(v1, v2, 30.0);
    EXPECT_TRUE(collisionResult.willCollide) << "垂直交叉路径的车辆应该检测到碰撞";
    
    // 测试2：不同速度比
    v1.speed = 5.0;   // 降低速度
    v2.speed = 15.0;  // 提高速度
    collisionResult = detector_->checkCollision(v1, v2, 30.0);
    EXPECT_TRUE(collisionResult.willCollide) << "不同速度的垂直交叉路径车辆应该检测到碰撞";
    
    // 测试3：超出碰撞检测范围
    v2.position = {200.0, 200.0};  // 增加到较远距离
    collisionResult = detector_->checkCollision(v1, v2, 30.0);
    EXPECT_FALSE(collisionResult.willCollide) << "距离较远时应该检测到碰撞";
}

// 测试静止辆的碰撞检测
TEST_F(CollisionDetectorTest, StationaryVehiclesCollision) {
    Vehicle v1;
    v1.vehicleNo = "VEH001";
    v1.position = {100.0, 100.0};
    v1.speed = 0.0;  // 静止
    v1.heading = 90.0;
    v1.type = MovingObjectType::UNMANNED;
    v1.isControllable = true;
    
    Vehicle v2;
    v2.vehicleNo = "VEH002";
    v2.position = {105.0, 100.0};  // 距离5米
    v2.speed = 0.0;  // 静止
    v2.heading = 90.0;
    v2.type = MovingObjectType::UNMANNED;
    v2.isControllable = true;
    
    // 测试1：两个静止车辆距离小于安全距离
    auto collisionResult = detector_->checkCollision(v1, v2, 30.0);
    EXPECT_TRUE(collisionResult.willCollide) << "距离小于安全距离的静止车辆应该检测到碰撞";
    
    // 测试2：一动一静，移动车辆接近静止车辆
    v2.position = {120.0, 100.0};  // 增加距离到20米
    v2.speed = 5.0;  // 开始移动
    v2.heading = 270.0;  // 向西行驶（朝向v1）
    collisionResult = detector_->checkCollision(v1, v2, 30.0);
    EXPECT_TRUE(collisionResult.willCollide) << "移动车辆接近静止车辆时应该检测到碰撞";
    
    // 测试3：一动一静，移动车辆远离静止车辆
    v2.position = {160.0, 100.0};  // 增加到60米，大于安全距离(50米)
    v2.speed = 5.0;  // 开始移动
    v2.heading = 90.0;  // 向东行驶（远离v1）
    collisionResult = detector_->checkCollision(v1, v2, 30.0);
    EXPECT_FALSE(collisionResult.willCollide) << "移动车辆远离静止车辆且距离于安全距离时不应该检测到碰撞";
}

// 测试同向运动的碰撞检测
TEST_F(CollisionDetectorTest, TailgatingCollision) {
    Vehicle v1;
    v1.vehicleNo = "VEH001";
    v1.position = {100.0, 100.0};
    v1.speed = 10.0;
    v1.heading = 90.0;  // 向东行驶
    v1.type = MovingObjectType::UNMANNED;
    v1.isControllable = true;
    
    Vehicle v2;
    v2.vehicleNo = "VEH002";
    v2.position = {40.0, 100.0};  // 在v1西侧60米处（100-60=40）
    v2.speed = 10.0;
    v2.heading = 90.0;  // 也向东行驶
    v2.type = MovingObjectType::UNMANNED;
    v2.isControllable = true;
    
    // 测试1：同向同速
    auto collisionResult = detector_->checkCollision(v1, v2, 30.0);
    EXPECT_FALSE(collisionResult.willCollide) << "同向同速的车辆不应该检测到碰撞";
    
    // 测试2：同向不同速（v2速度更快，会追上v1）
    v2.speed = 15.0;
    collisionResult = detector_->checkCollision(v1, v2, 30.0);
    EXPECT_TRUE(collisionResult.willCollide) << "同向但速度较快的车辆追上前车时应该检测到碰撞";
    
    // 测试3：同向不同速但距离较远
    v2.position = {0.0, 100.0};  // 在原点
    v2.speed = 11.0;            // 减小速度差，从 15m/s 改为 11m/s
    collisionResult = detector_->checkCollision(v1, v2, 30.0);
    EXPECT_FALSE(collisionResult.willCollide) << "相对速度小且距离较远时不应该检测到碰撞";
}

// 测试航空器与静止车辆的碰撞检测
TEST_F(CollisionDetectorTest, AircraftStationaryVehicleCollision) {
    Aircraft aircraft;
    aircraft.flightNo = "TEST001";
    aircraft.position = {100.0, 100.0};
    aircraft.speed = 15.0;
    aircraft.heading = 90.0;  // 向东行驶
    aircraft.type = MovingObjectType::AIRCRAFT;
    
    Vehicle vehicle;
    vehicle.vehicleNo = "VEH001";
    vehicle.position = {150.0, 100.0};  // 在航空器前方50米处
    vehicle.speed = 0.0;  // 静止
    vehicle.heading = 90.0;
    vehicle.type = MovingObjectType::UNMANNED;
    vehicle.isControllable = true;
    
    // 测试1：航空器接近静止车辆
    auto collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
    EXPECT_TRUE(collisionResult.willCollide) << "航空器接近静止车辆时应该检测到碰撞";
    
    // 测试2：静止车辆在航空器航向偏离处
    vehicle.position = {200.0, 200.0};  // 在航空器前方偏北，距离约100米（大于安全距离75米）
    collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
    EXPECT_FALSE(collisionResult.willCollide) << "航空器与不在航向上的静止车辆不应该检测到碰撞";
    
    // 测试3：静止车辆在航空器后方
    vehicle.position = {0.0, 100.0};  // 在航空器后方100米（大于安全距离75米）
    collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
    EXPECT_FALSE(collisionResult.willCollide) << "航空器与后方的静止车辆不应该检测到碰撞";
}