#include "System.h"
#include "collector/DataCollector.h"
#include "detector/CollisionDetector.h"
#include "command/CommandSender.h"
#include <chrono>
#include <thread>

CollisionAvoidanceSystem::CollisionAvoidanceSystem()
    : dataCollector_(std::make_unique<DataCollector>())
    , collisionDetector_(std::make_unique<CollisionDetector>())
    , commandSender_(std::make_unique<CommandSender>())
    , running_(false) {
}

void CollisionAvoidanceSystem::start() {
    running_ = true;
    processLoop();
}

void CollisionAvoidanceSystem::stop() {
    running_ = false;
}

void CollisionAvoidanceSystem::processLoop() {
    while (running_) {
        // 获取数据
        if (auto data = dataCollector_->getData()) {
            // 检测碰撞
            auto result = collisionDetector_->detect(*data, {});
            
            // 如果检测到碰撞风险，发送避障指令
            if (result.hasCollision) {
                VehicleCommand cmd{
                    .vehicleId = data->id,
                    .type = CommandType::STOP,
                    .avoidanceVector = result.avoidanceVector
                };
                commandSender_->sendCommand(cmd, CommandSender::Priority::HIGH);
            }
        }
        
        // 控制处理频率
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}