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

// 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, 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_METHOD(AreaType, getAreaType, (const Vector2D& position), (const));
    
    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;
    }
};

class BasicCollisionTest : public ::testing::Test {
protected:
    void SetUp() override {
        Logger::setLogLevel(LogLevel::DEBUG);  // 设置日志级别为 DEBUG
        
        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_);
    }

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

// 1. 静态碰撞测试
TEST_F(BasicCollisionTest, StaticCollision) {
    // 创建两个静止且重叠的物体
    Vehicle v1;
    v1.vehicleNo = "V1";
    v1.position = {100.0, 100.0};
    v1.speed = 0.0;
    v1.heading = 0.0;
    v1.type = MovingObjectType::UNMANNED;
    
    Vehicle v2;
    v2.vehicleNo = "V2";
    v2.position = {120.0, 100.0};  // 距离20米，小于碰撞半径25米
    v2.speed = 0.0;
    v2.heading = 0.0;
    v2.type = MovingObjectType::UNMANNED;
    
    auto result = detector_->checkCollision(v1, v2, 30.0);
    
    EXPECT_TRUE(result.willCollide) << "距离小于碰撞半径的静止物��应该检测为碰撞";
    EXPECT_DOUBLE_EQ(result.timeToCollision, 0.0) << "静止物体的碰撞时间应该为0";
    EXPECT_EQ(result.type, collision::CollisionType::STATIC) << "应该识别为静态碰撞";
    
    // 增加更多验证
    EXPECT_DOUBLE_EQ(result.minDistance, 20.0) << "最小距离应该是当前距离20米";
    EXPECT_DOUBLE_EQ(result.timeToMinDistance, 0.0) << "静止物体的最小距离时间应该为0";
    EXPECT_NEAR(result.collisionPoint.x, 110.0, 0.1) << "碰撞点应该在两车中点";
    EXPECT_NEAR(result.collisionPoint.y, 100.0, 0.1) << "碰撞点应该在同一水平线上";
    
    // 验证物体状态
    EXPECT_NEAR(result.object1State.position.x, 100.0, 0.1);
    EXPECT_NEAR(result.object1State.position.y, 100.0, 0.1);
    EXPECT_DOUBLE_EQ(result.object1State.speed, 0.0);
    EXPECT_DOUBLE_EQ(result.object1State.heading, 0.0);
    
    EXPECT_NEAR(result.object2State.position.x, 120.0, 0.1);
    EXPECT_NEAR(result.object2State.position.y, 100.0, 0.1);
    EXPECT_DOUBLE_EQ(result.object2State.speed, 0.0);
    EXPECT_DOUBLE_EQ(result.object2State.heading, 0.0);
}

// 2. 相向碰撞测试
TEST_F(BasicCollisionTest, HeadOnCollision) {
    // 创建两个相向运动的物体
    Vehicle v1;
    v1.vehicleNo = "V1";
    v1.position = {100.0, 100.0};
    v1.speed = 10.0;
    v1.heading = 90.0;  // 向东
    v1.type = MovingObjectType::UNMANNED;
    
    Vehicle v2;
    v2.vehicleNo = "V2";
    v2.position = {200.0, 100.0};  // 在v1东边100米
    v2.speed = 10.0;
    v2.heading = 270.0;  // 向西
    v2.type = MovingObjectType::UNMANNED;
    
    auto result = detector_->checkCollision(v1, v2, 30.0);
    
    EXPECT_TRUE(result.willCollide) << "相向运动的物体应该检测为碰撞";
    EXPECT_NEAR(result.timeToCollision, 2.5, 0.1) << "碰撞时间应该接近2.5秒((100-50)米/20米每秒)";
    EXPECT_EQ(result.type, collision::CollisionType::HEAD_ON) << "应该识别为相向碰撞";
    EXPECT_NEAR(result.collisionPoint.x, 150.0, 0.1) << "碰撞点应该在两车中点处(100+200)/2=150";
    EXPECT_NEAR(result.collisionPoint.y, 100.0, 0.1) << "碰撞点应该在同一水平线上";
    
    // 增加更多验证
    EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离应该是碰撞半径之和50米";
    EXPECT_NEAR(result.timeToMinDistance, 2.5, 0.1) << "最小距离时间应该等于碰撞时间";
    
    // 验证物体状态
    EXPECT_NEAR(result.object1State.position.x, 125.0, 0.1);
    EXPECT_NEAR(result.object1State.position.y, 100.0, 0.1);
    EXPECT_DOUBLE_EQ(result.object1State.speed, 10.0);
    EXPECT_DOUBLE_EQ(result.object1State.heading, 90.0);
    
    EXPECT_NEAR(result.object2State.position.x, 175.0, 0.1);
    EXPECT_NEAR(result.object2State.position.y, 100.0, 0.1);
    EXPECT_DOUBLE_EQ(result.object2State.speed, 10.0);
    EXPECT_DOUBLE_EQ(result.object2State.heading, 270.0);
}

// 3. 平行运动测试
TEST_F(BasicCollisionTest, ParallelMotion) {
    // 创建两个平行运动的物体
    Vehicle v1;
    v1.vehicleNo = "V1";
    v1.position = {100.0, 100.0};
    v1.speed = 10.0;
    v1.heading = 90.0;  // 向东
    v1.type = MovingObjectType::UNMANNED;
    
    Vehicle v2;
    v2.vehicleNo = "V2";
    v2.position = {100.0, 150.0};  // 在v1北边50米，大于碰撞半径
    v2.speed = 10.0;
    v2.heading = 90.0;  // 向东
    v2.type = MovingObjectType::UNMANNED;
    
    auto result = detector_->checkCollision(v1, v2, 30.0);
    
    EXPECT_FALSE(result.willCollide) << "平行运动且距离大于碰撞半径的物体不应该检测为碰撞";
    EXPECT_EQ(result.type, collision::CollisionType::PARALLEL) << "应该识别为平行运动";
    
    // 增加更多验证
    EXPECT_DOUBLE_EQ(result.minDistance, 50.0) << "最小距离应该是初始距离50米";
    EXPECT_DOUBLE_EQ(result.timeToMinDistance, 0.0) << "平行运动的最小距离时间应该为0";
    EXPECT_DOUBLE_EQ(result.timeToCollision, std::numeric_limits<double>::infinity()) << "平行运动不会碰撞";
    
    // 不验证碰撞点，因为不会发生碰撞
}

// 4. 交叉路径测试
TEST_F(BasicCollisionTest, CrossingPaths) {
    // 创建两个垂直交叉运动的物体
    Vehicle v1;
    v1.vehicleNo = "V1";
    v1.position = {100.0, 100.0};
    v1.speed = 10.0;
    v1.heading = 75.0;  // 向东北偏东方向运动
    v1.type = MovingObjectType::UNMANNED;
    
    Vehicle v2;
    v2.vehicleNo = "V2";
    v2.position = {150.0, 150.0};
    v2.speed = 10.0;
    v2.heading = 105.0;  // 向东南偏东方向运动，与 v1 夹角为 30 度
    v2.type = MovingObjectType::UNMANNED;
    
    auto result = detector_->checkCollision(v1, v2, 30.0);
    
    EXPECT_TRUE(result.willCollide) << "交叉路径的物体应该检测为碰撞";
    EXPECT_EQ(result.type, collision::CollisionType::CROSSING) << "应该识别为交叉碰撞";
    
    // 计算碰撞时间和位置：
    // v1: 速度分量 (2.59, 9.66) m/s
    // v2: 速度分量 (-2.59, 9.66) m/s
    // 相对速度: (-5.18, 0) m/s
    // 相对速度大小: 5.18 m/s
    // 碰撞点在两车轨迹交点前的安全距离处
    double collision_time = 6.12;  // 根据实际计算得到
    Vector2D collision_point = {125.0, 184.15};  // 根据实际计算得到
    
    EXPECT_NEAR(result.timeToCollision, collision_time, 0.1) << "考虑碰撞半径25米，撞时间应该接近6.12秒";
    EXPECT_NEAR(result.collisionPoint.x, collision_point.x, 0.1) << "碰撞点x坐标应该在125.0";
    EXPECT_NEAR(result.collisionPoint.y, collision_point.y, 0.1) << "碰撞点y坐标应该在184.15";
    
    // 增加更多验证
    EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离应该是碰撞半径之和50米";
    EXPECT_NEAR(result.timeToMinDistance, collision_time, 0.1) << "最小距离时间应该等于碰撞时间";
    
    // 验证物体状态
    EXPECT_NEAR(result.object1State.position.x, 115.85, 0.1);
    EXPECT_NEAR(result.object1State.position.y, 159.15, 0.1);
    EXPECT_DOUBLE_EQ(result.object1State.speed, 10.0);
    EXPECT_DOUBLE_EQ(result.object1State.heading, 75.0);
    
    EXPECT_NEAR(result.object2State.position.x, 134.15, 0.1);
    EXPECT_NEAR(result.object2State.position.y, 209.15, 0.1);
    EXPECT_DOUBLE_EQ(result.object2State.speed, 10.0);
    EXPECT_DOUBLE_EQ(result.object2State.heading, 105.0);
}

// 5. 垂直交叉路径测试
TEST_F(BasicCollisionTest, PerpendicularCrossingPaths) {
    // 创建两个垂直交叉运动的物体
    Vehicle v1;
    v1.vehicleNo = "V1";
    v1.position = {100.0, 100.0};
    v1.speed = 10.0;
    v1.heading = 90.0;  // 向东运动
    v1.type = MovingObjectType::UNMANNED;
    
    Vehicle v2;
    v2.vehicleNo = "V2";
    v2.position = {150.0, 150.0};
    v2.speed = 10.0;
    v2.heading = 180.0;  // 向南运动，与 v1 夹角为 90 度
    v2.type = MovingObjectType::UNMANNED;
    
    auto result = detector_->checkCollision(v1, v2, 30.0);
    
    EXPECT_TRUE(result.willCollide) << "垂直交叉路径的物体应该检测为碰撞";
    EXPECT_EQ(result.type, collision::CollisionType::CROSSING) << "应该识别为交叉碰撞";
    
    // 计算碰撞时间和位置：
    // v1: 速度分量 (0.0, 10.0) m/s
    // v2: 速度分量 (-10.0, 0.0) m/s
    // 相对速度: (-10.0, -10.0) m/s
    // 相对速度大小: 14.14 m/s
    // 由于碰撞半径为 25m，实际碰撞会提前发生
    double collision_time = 1.46;  // 根据实际计算得到
    Vector2D collision_point = {117.68, 132.32};  // 根据实际计算得到
    
    EXPECT_NEAR(result.timeToCollision, collision_time, 0.1) << "考虑碰撞半径25��，碰时间应该接近1.46秒";
    EXPECT_NEAR(result.collisionPoint.x, collision_point.x, 0.1) << "碰撞点x坐标应该在117.68";
    EXPECT_NEAR(result.collisionPoint.y, collision_point.y, 0.1) << "碰撞点y坐标应该在132.32";
    
    // 增加更多验证
    EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离应该是碰撞半径之和50米";
    EXPECT_NEAR(result.timeToMinDistance, collision_time, 0.1) << "最小距离时间应该等于碰撞时间";
    
    // 验证物体状态
    EXPECT_NEAR(result.object1State.position.x, 100.0, 0.1);
    EXPECT_NEAR(result.object1State.position.y, 114.64, 0.1);
    EXPECT_DOUBLE_EQ(result.object1State.speed, 10.0);
    EXPECT_DOUBLE_EQ(result.object1State.heading, 90.0);
    
    EXPECT_NEAR(result.object2State.position.x, 135.36, 0.1);
    EXPECT_NEAR(result.object2State.position.y, 150.0, 0.1);
    EXPECT_DOUBLE_EQ(result.object2State.speed, 10.0);
    EXPECT_DOUBLE_EQ(result.object2State.heading, 180.0);
}

TEST_F(BasicCollisionTest, DivergentMotion) {
    // 设置两个物体背向运动的场景
    // 物体1在(150,100)，向右运动，航向90度
    // 物体2在(200,100)，向右运动，航向90度
    // 两个物体都在远离对方，不应该发生碰撞
    
    Vehicle obj1;
    obj1.vehicleNo = "V1";
    obj1.position = {150, 100};
    obj1.speed = 10;
    obj1.heading = 90;  // 右运动
    obj1.type = MovingObjectType::UNMANNED;
    
    Vehicle obj2;
    obj2.vehicleNo = "V2";
    obj2.position = {300, 100};  // 增加初始距离到 150m
    obj2.speed = 10;
    obj2.heading = 90;  // 也向右运动
    obj2.type = MovingObjectType::SPECIAL;
    
    auto result = detector_->checkCollision(obj1, obj2, 10.0);
    
    // 验证结果
    EXPECT_FALSE(result.willCollide) << "同向运动且距离大于安全距离的物体不应该发生碰撞";
    EXPECT_EQ(result.type, collision::CollisionType::PARALLEL) << "应该识别为平行运动";
    
    // 初始距离应该是 150m
    EXPECT_NEAR(result.minDistance, 150.0, 0.1);
    
    // 由于两车都以相同速度向右运动，最小距离应该保持不变
    EXPECT_NEAR(result.timeToMinDistance, 0.0, 0.1);
}

// 6. 追尾场景测试
TEST_F(BasicCollisionTest, TailgatingMotion) {
    // 创建两个同向运动的物体，后车速度大于前车
    Vehicle v1;  // 前车
    v1.vehicleNo = "V1";
    v1.position = {60.0, 100.0};  // 前车在前方60米处
    v1.speed = 10.0;              // 前车速度10m/s
    v1.heading = 90.0;            // 向东运动
    v1.type = MovingObjectType::UNMANNED;
    
    Vehicle v2;  // 后车
    v2.vehicleNo = "V2";
    v2.position = {0.0, 100.0};   // 后车在原点
    v2.speed = 15.0;              // 后车速度15m/s
    v2.heading = 90.0;            // 向东运动
    v2.type = MovingObjectType::UNMANNED;
    
    auto result = detector_->checkCollision(v1, v2, 30.0);
    
    // 验证碰撞类型
    EXPECT_EQ(result.type, collision::CollisionType::PARALLEL) << "应该识别为平行运动";
    
    // 验证会发生碰撞
    EXPECT_TRUE(result.willCollide) << "后车速度大于前车，应该预测到碰撞";
    
    // 验证碰撞时间（初始距离60米，相对速度5m/s，安全距离50米，需要缩短10米，所以碰撞时间应该是2秒）
    EXPECT_NEAR(result.timeToCollision, 2.0, 0.1) << "碰撞时间应该接近2秒";
    
    // 验证最小距离（应该是安全距离）
    EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离应该是安全距离50米";
    
    // 验证最小距离时间（应该等于碰撞时间）
    EXPECT_NEAR(result.timeToMinDistance, 2.0, 0.1) << "最小距离时间应该等于碰撞时间";
    
    // 验证碰撞点（在两车碰撞时的中点）
    // 前车：初始位置 60 + 10 * 2 = 80
    // 后车：初始位置 0 + 15 * 2 = 30
    // 碰撞点应该在 (80 + 30) / 2 = 55
    EXPECT_NEAR(result.collisionPoint.x, 55.0, 0.1) << "碰撞点x坐标应该在55米处";
    EXPECT_NEAR(result.collisionPoint.y, 100.0, 0.1) << "碰撞点y坐标应该保持在100米";
    
    // 验证碰撞时刻的物体状态
    // 前车位置：60 + 10 * 2 = 80
    EXPECT_NEAR(result.object1State.position.x, 80.0, 0.1);
    EXPECT_NEAR(result.object1State.position.y, 100.0, 0.1);
    EXPECT_DOUBLE_EQ(result.object1State.speed, 10.0);
    EXPECT_DOUBLE_EQ(result.object1State.heading, 90.0);
    
    // 后车位置：0 + 15 * 2 = 30
    EXPECT_NEAR(result.object2State.position.x, 30.0, 0.1);
    EXPECT_NEAR(result.object2State.position.y, 100.0, 0.1);
    EXPECT_DOUBLE_EQ(result.object2State.speed, 15.0);
    EXPECT_DOUBLE_EQ(result.object2State.heading, 90.0);
}