/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — cài đặt MissionAdapterBridge.
 *
 * Author: DuongTD
 *********************************************************************/
#include <move_base2/bridges/mission_adapter_bridge.h>

#include <utility>

#include <mission_adapters/mission_manager.h>
#include <robot/robot.h>

namespace move_base2
{

MissionAdapterBridge::MissionAdapterBridge() = default;
MissionAdapterBridge::~MissionAdapterBridge() = default;

void MissionAdapterBridge::attach(mission_adapters::MissionManager* manager)
{
  std::lock_guard<std::mutex> lock(mutex_);
  manager_ = manager;
}

void MissionAdapterBridge::setCancelCallback(CancelCallback callback)
{
  std::lock_guard<std::mutex> lock(mutex_);
  cancel_callback_ = std::move(callback);
}

void MissionAdapterBridge::setRequestCallback(RequestCallback callback)
{
  std::lock_guard<std::mutex> lock(mutex_);
  request_callback_ = std::move(callback);
}

// ================================================================================================
// Chuyển đổi
// ================================================================================================

NavigationRequest MissionAdapterBridge::toRequest(const mission_adapters::Mission& mission)
{
  NavigationRequest request;

  request.mission_sequence_id = mission.id;
  request.has_goal = mission.has_goal;
  request.goal = mission.goal;

  // Sai số để mặc định: quy ước của NavigationRequest là giá trị <= 0 nghĩa "dùng default của
  // profile trong config". Mission layer không biết gì về sai số hình học nên không được đặt.
  request.tolerance = GoalTolerance();

  // Mission layer KHÔNG diễn giải actionType và không lọc gì (D8) — action đi qua nguyên vẹn, đúng
  // thứ tự đã sắp theo sequenceId.
  request.actions.reserve(mission.actions.size());
  for (const auto& action : mission.actions)
  {
    request.actions.push_back(action.action);
  }

  // Mọi mission đều chạy profile position. `MissionType` chỉ nói mission đến TỪ ĐÂU (goal đơn hay
  // order VDA5050), không nói robot phải di chuyển KIỂU gì — docking/go-straight/rotate là lựa chọn
  // của người vận hành qua sáu entry point của contract host, không phải của mission layer.
  request.profile = MotionProfile::kPosition;

  return request;
}

// ================================================================================================
// NavigationClient — gọi từ thread của MissionExecutor
// ================================================================================================

bool MissionAdapterBridge::dispatch(const std::shared_ptr<const mission_adapters::Mission>& mission)
{
  if (!mission)
  {
    robot::log_error("[move_base2] MissionAdapterBridge: dispatch(nullptr).\n");
    return false;
  }

  std::lock_guard<std::mutex> lock(mutex_);

  if (!running_)
  {
    // Chưa start hoặc đã stop. Từ chối thay vì cất lại: mission layer phải biết chặng của nó không
    // được nhận, chứ không phải chờ một kết quả sẽ không bao giờ tới.
    robot::log_warning("[move_base2] MissionAdapterBridge: từ chối mission %llu — bridge chưa "
                       "start.\n", static_cast<unsigned long long>(mission->id));
    return false;
  }

  if (pending_)
  {
    // Không nên xảy ra: MissionManager chỉ giao chặng mới sau khi chặng cũ kết thúc. Đếm lại thay
    // vì im lặng — một chặng biến mất trong khi fleet master vẫn chờ nó là lỗi rất khó truy.
    ++dropped_requests_;
    robot::log_warning("[move_base2] MissionAdapterBridge: mission %llu đè mission %llu chưa kịp "
                       "đẩy xuống.\n", static_cast<unsigned long long>(mission->id),
                       static_cast<unsigned long long>(pending_->id));
  }

  // Chỉ cất lại. Chuyển đổi và đẩy xuống navigation xảy ra ở pumpPendingRequest(), trên control
  // thread — xem doc của lớp.
  pending_ = mission;
  return true;
}

void MissionAdapterBridge::cancelActive(mission_adapters::MissionId /*id*/)
{
  CancelCallback callback;
  {
    std::lock_guard<std::mutex> lock(mutex_);

    // Chặng đang chờ mà chưa kịp xuống navigation thì huỷ ngay tại đây: đẩy nó xuống rồi mới huỷ là
    // cho robot nhúc nhích một cycle vì một chặng đã bị thu hồi.
    pending_.reset();
    callback = cancel_callback_;
  }

  // Gọi NGOÀI lock: callback đi vào control loop, không được chạy dưới mutex của bridge.
  if (callback)
  {
    callback();
  }
}

// ================================================================================================
// Biên thread — chỉ control thread gọi
// ================================================================================================

bool MissionAdapterBridge::pumpPendingRequest()
{
  std::shared_ptr<const mission_adapters::Mission> mission;
  RequestCallback callback;

  {
    std::lock_guard<std::mutex> lock(mutex_);
    if (!pending_ || !request_callback_)
    {
      return false;
    }
    mission = pending_;
    pending_.reset();
    callback = request_callback_;
  }

  // Ngoài lock: callback đi thẳng vào ControlLoop::submit.
  callback(toRequest(*mission));
  return true;
}

// ================================================================================================
// MissionPort — gọi từ control thread
// ================================================================================================

void MissionAdapterBridge::reportOutcome(std::uint64_t mission_sequence_id,
                                         NavigationOutcome outcome)
{
  if (mission_sequence_id == mission_adapters::kInvalidMissionId)
  {
    // Goal trực tiếp từ contract host, không thuộc mission nào. Không có gì để báo.
    return;
  }

  mission_adapters::MissionManager* manager = nullptr;
  {
    std::lock_guard<std::mutex> lock(mutex_);
    manager = manager_;
  }

  if (manager == nullptr)
  {
    return;
  }

  // Chỉ kSucceeded mới là "chặng xong". Ba kết cục còn lại đều là "chặng không hoàn thành", và
  // chính sách hàng đợi (`clear_queue_on_failure`) nằm ở mission layer chứ không ở đây.
  //
  // kPreempted đáng chú ý: mission layer thường đã chuyển sang chặng khác rồi, nên
  // onNavigationFailed sẽ trả false và không làm gì — đúng ý, chứ không phải bị bỏ sót.
  const bool accepted = (outcome == NavigationOutcome::kSucceeded)
                            ? manager->onNavigationDone(mission_sequence_id)
                            : manager->onNavigationFailed(mission_sequence_id);

  if (!accepted)
  {
    std::lock_guard<std::mutex> lock(mutex_);
    ++stale_outcomes_;
  }
}

bool MissionAdapterBridge::hasActiveMission() const
{
  mission_adapters::MissionManager* manager = nullptr;
  bool has_pending = false;
  {
    std::lock_guard<std::mutex> lock(mutex_);
    manager = manager_;
    has_pending = static_cast<bool>(pending_);
  }

  if (has_pending)
  {
    return true;
  }
  return manager != nullptr && manager->hasMission();
}

void MissionAdapterBridge::start()
{
  std::lock_guard<std::mutex> lock(mutex_);
  running_ = true;
}

void MissionAdapterBridge::stop()
{
  std::lock_guard<std::mutex> lock(mutex_);
  running_ = false;
  // Bỏ chặng đang chờ: nó sẽ không bao giờ được chạy, và giữ lại chỉ để nó chạy sau một lần start()
  // sau này là hành vi không ai mong đợi.
  pending_.reset();
}

std::size_t MissionAdapterBridge::droppedRequests() const
{
  std::lock_guard<std::mutex> lock(mutex_);
  return dropped_requests_;
}

std::size_t MissionAdapterBridge::staleOutcomes() const
{
  std::lock_guard<std::mutex> lock(mutex_);
  return stale_outcomes_;
}

}  // namespace move_base2