#include "detector/TrafficLightDetector.h"
#include "utils/Logger.h"
#include "types/BasicTypes.h"
#include "core/System.h"
#include <chrono>

TrafficLightDetector::TrafficLightDetector(const IntersectionConfig& intersectionConfig,
    ControllableVehicles& controllableVehicles,
    System& system)
    : intersection_config_(intersectionConfig)
    , controllable_vehicles_(controllableVehicles)
    , system_(system) {}

void TrafficLightDetector::processSignal(const TrafficLightSignal& signal,
    const std::vector<Vehicle>& vehicles) {
    // 根据红绿灯ID查找对应的路口
    const Intersection* intersection = intersection_config_.findByTrafficLightId(signal.trafficLightId);
    if (!intersection) {
        Logger::warning("未找到红绿灯对应的路口: ", signal.trafficLightId);
        return;
    }

    // 保存当前处理的信号
    current_signal_ = signal;

    // 检查每个车辆
    for (const auto& vehicle : vehicles) {
        if (controllable_vehicles_.isControllable(vehicle.vehicleNo)) {
            // 根据信号灯状态发送指令
            // FIXME: 临时修改 - 仅根据南北向状态发送指令，未考虑车辆方向和东西向状态。
            // 需要根据车辆行驶方向判断应该参考 ns_status 还是 ew_status。
            switch (signal.ns_status) {
            case SignalStatus::RED:
                sendSignalCommand(vehicle.vehicleNo, SignalState::RED);
                break;
            case SignalStatus::GREEN:
                sendSignalCommand(vehicle.vehicleNo, SignalState::GREEN);
                break;
            case SignalStatus::YELLOW:
                sendSignalCommand(vehicle.vehicleNo, SignalState::YELLOW);
                break;
            default:
                Logger::warning("未知的信号灯状态");
                break;
            }
        }
    }
}

void TrafficLightDetector::sendSignalCommand(const std::string& vehicleNo, SignalState state) {
    VehicleCommand cmd;
    cmd.vehicleId = vehicleNo;
    cmd.type = CommandType::SIGNAL;
    cmd.reason = CommandReason::TRAFFIC_LIGHT;
    cmd.signalState = state;
    cmd.timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count();

    // 添加路口信息
    const Intersection* intersection = intersection_config_.findByTrafficLightId(current_signal_.trafficLightId);
    if (intersection) {
        cmd.intersectionId = intersection->id;
        cmd.latitude = intersection->position.latitude;
        cmd.longitude = intersection->position.longitude;
    }

    controllable_vehicles_.sendCommand(vehicleNo, cmd);
    system_.broadcastVehicleCommand(cmd);
    Logger::debug("发送信号灯指令到车辆: ", vehicleNo,
        " 路口: ", cmd.intersectionId,
        " 状态: ", state == SignalState::RED ? "红灯" : state == SignalState::GREEN ? "绿灯" : "黄灯");
}