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

namespace collision {

SafetyZone::SafetyZone(const Vector2D& center, double aircraftRadius, double specialVehicleRadius)
    : center_(center)
    , aircraftRadius_(aircraftRadius)
    , specialVehicleRadius_(specialVehicleRadius)
    , currentRadius_(0.0)
    , state_(SafetyZoneState::INACTIVE)
    , type_(SafetyZoneType::NONE) {
    
    Logger::debug("创建安全区: center=(", center_.x, ",", center_.y, 
                 "), aircraftRadius=", aircraftRadius_,
                 ", specialVehicleRadius=", specialVehicleRadius_);
}

bool SafetyZone::isInZone(const Vector2D& point) const {
    // 如果未设置类型，直接返回 false
    if (type_ == SafetyZoneType::NONE) {
        return false;
    }
    
    // 计算点到中心的距离
    double dx = point.x - center_.x;
    double dy = point.y - center_.y;
    double distance = std::sqrt(dx * dx + dy * dy);
    
    // 使用当前设置的安全区半径
    return distance <= currentRadius_;
}

void SafetyZone::resetType() {
    type_ = SafetyZoneType::NONE;
    currentRadius_ = 0.0;
    Logger::debug("重置安全区类型为 NONE");
}

bool SafetyZone::trySetType(const MovingObject& object) {
    // 判断目标类型
    bool isAircraft = object.isAircraft();
    bool isSpecialVehicle = object.isSpecialVehicle();
    
    // 如果既不是飞机也不是特勤车，返回 false
    if (!isAircraft && !isSpecialVehicle) {
        return false;
    }
    
    // 如果安全区已有类型，检查是否可以改变
    if (type_ != SafetyZoneType::NONE) {
        // 如果当前类型与目标类型不匹配，不允许改变
        if ((type_ == SafetyZoneType::AIRCRAFT && !isAircraft) ||
            (type_ == SafetyZoneType::VEHICLE && !isSpecialVehicle)) {
            return false;
        }
        // 如果类型匹配，保持当前类型
        return true;
    }
    
    // 设置新的类型和尺寸
    if (isAircraft) {
        type_ = SafetyZoneType::AIRCRAFT;
        currentRadius_ = aircraftRadius_;
        state_ = SafetyZoneState::ACTIVE;  // 激活安全区
        Logger::debug("安全区设置为飞机类型，半径: ", currentRadius_);
    } else {
        type_ = SafetyZoneType::VEHICLE;
        currentRadius_ = specialVehicleRadius_;
        state_ = SafetyZoneState::ACTIVE;  // 激活安全区
        Logger::debug("安全区设置为特勤车类型，半径: ", currentRadius_);
    }
    
    return true;
}

bool SafetyZone::isObjectInZone(const MovingObject& object) const {
    // 如果是无人车，直接返回 false
    if (object.isUnmannedVehicle()) {
        Logger::debug("目标 ", object.id, " 是无人车，不触发安全区");
        return false;
    }
    
    // 从 SystemConfig 获取物体尺寸（半长度）
    const auto& config = SystemConfig::instance().collision_detection.prediction;
    double objectRadius = 0.0;
    
    // 获取物体半径
    if (object.isAircraft()) {
        objectRadius = config.aircraft_size / 2.0;
        Logger::debug("目标 ", object.id, " 是飞机，尺寸半径: ", objectRadius);
    } else if (object.isSpecialVehicle()) {
        objectRadius = config.vehicle_size / 2.0;
        Logger::debug("目标 ", object.id, " 是特勤车，尺寸半径: ", objectRadius);
    } else {
        return false;
    }
    
    // 如果安全区类型未设置，尝试设置类型
    if (type_ == SafetyZoneType::NONE) {
        // 临时设置类型和半径来检查是否在区内
        SafetyZoneType tempType = object.isAircraft() ? SafetyZoneType::AIRCRAFT : SafetyZoneType::VEHICLE;
        double tempRadius = object.isAircraft() ? aircraftRadius_ : specialVehicleRadius_;
        
        // 暂存当前值
        auto currentType = type_;
        auto currentRadius = currentRadius_;
        
        // 临时设置类型和半径
        const_cast<SafetyZone*>(this)->type_ = tempType;
        const_cast<SafetyZone*>(this)->currentRadius_ = tempRadius;
        
        // 检查是否在区内
        bool inZone = isInZone(object.position);
        
        // 如果在区内，保持新的类型和半径，否则恢复原值
        if (!inZone) {
            const_cast<SafetyZone*>(this)->type_ = currentType;
            const_cast<SafetyZone*>(this)->currentRadius_ = currentRadius;
        } else {
            // 设置状态为激活
            const_cast<SafetyZone*>(this)->state_ = SafetyZoneState::ACTIVE;
            Logger::debug("安全区设置为", object.isAircraft() ? "飞机" : "特勤车", "类型，半径: ", tempRadius);
        }
        
        return inZone;
    }
    
    // 如果类型已设置，检查是否匹配
    if ((type_ == SafetyZoneType::AIRCRAFT && !object.isAircraft()) ||
        (type_ == SafetyZoneType::VEHICLE && !object.isSpecialVehicle())) {
        Logger::debug("目标 ", object.id, " 类型与安全区类型不匹配，当前安全区类型: ", 
                     type_ == SafetyZoneType::AIRCRAFT ? "飞机" : "特勤车");
        return false;
    }
    
    // 使用 isInZone 检查是否在区内
    return isInZone(object.position);
}

} // namespace collision