// Copyright chenyy 2026. Licensed under the MIT License.
// Async detector implementation
//
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "armor_yolo_detect/async_detector.hpp"

namespace armor_yolo_detect {

AsyncDetector::AsyncDetector() = default;

AsyncDetector::~AsyncDetector() {
  stop();
}

void AsyncDetector::init(std::shared_ptr<Detector> detector, int queue_size) {
  detector_ = detector;
  max_queue_size_ = static_cast<size_t>(std::max(1, queue_size));
}

void AsyncDetector::start() {
  if (running_) {
    return;
  }

  running_ = true;
  processing_thread_ = std::thread(&AsyncDetector::processingLoop, this);
}

void AsyncDetector::stop() {
  if (!running_) {
    return;
  }

  running_ = false;
  queue_cv_.notify_one();

  if (processing_thread_.joinable()) {
    processing_thread_.join();
  }
}

void AsyncDetector::enqueue(const cv::Mat& img) {
  std::lock_guard<std::mutex> lock(queue_mutex_);

  // Remove oldest frame if queue is full
  if (frame_queue_.size() >= max_queue_size_) {
    frame_queue_.pop_front();
  }

  static uint64_t frame_id_counter = 0;
  AsyncFrame frame;
  frame.img = img.clone();
  frame.frame_id = frame_id_counter++;
  frame.timestamp = std::chrono::steady_clock::now();

  frame_queue_.push_back(std::move(frame));
  queue_cv_.notify_one();
}

std::vector<fyt::auto_aim::Armor> AsyncDetector::getLatestResult() {
  std::lock_guard<std::mutex> lock(result_mutex_);
  return latest_result_;
}

void AsyncDetector::processingLoop() {
  while (running_) {
    AsyncFrame frame;

    {
      std::unique_lock<std::mutex> lock(queue_mutex_);
      queue_cv_.wait_for(
          lock,
          std::chrono::milliseconds(100),
          [this]() { return !frame_queue_.empty() || !running_; });

      if (!running_ && frame_queue_.empty()) {
        break;
      }

      if (frame_queue_.empty()) {
        continue;
      }

      frame = std::move(frame_queue_.front());
      frame_queue_.pop_front();
    }

    // Process frame
    if (detector_) {
      auto result = detector_->detect(frame.img);

      {
        std::lock_guard<std::mutex> lock(result_mutex_);
        latest_result_ = result;
      }
    }
  }
}

}  // namespace armor_yolo_detect
