#include "robot/robot_runtime.hpp"

#include "mission/mission_queue.hpp"
#include "storage/database.hpp"
#include "util/id_util.hpp"

#include <algorithm>
#include <chrono>
#include <cmath>

namespace lm {

namespace {

constexpr const char* kDefaultMessage = "Waiting for new missions...";

}  // namespace

RobotRuntime::RobotRuntime(Database& db, MissionQueue& mission_queue)
    : db_(db), mission_queue_(mission_queue)
{
  load();
  ensureDefaultsUnlocked();
}

void RobotRuntime::load()
{
  std::lock_guard<std::mutex> lock(mu_);
  state_ = nlohmann::json::object();
  nlohmann::json parsed;
  if (db_.getDocument("robot_runtime", parsed) && parsed.is_object())
    state_ = parsed;
}

void RobotRuntime::saveUnlocked() const
{
  db_.setDocument("robot_runtime", state_);
}

void RobotRuntime::ensureDefaultsUnlocked()
{
  if (!state_.is_object())
    state_ = nlohmann::json::object();
  if (!state_.contains("motion"))
    state_["motion"] = "paused";
  if (!state_.contains("health"))
    state_["health"] = "ok";
  if (!state_.contains("message"))
    state_["message"] = kDefaultMessage;
  if (!state_.contains("error"))
    state_["error"] = nullptr;
  if (!state_.contains("battery_percent"))
    state_["battery_percent"] = 54;
  if (!state_.contains("battery_charging"))
    state_["battery_charging"] = false;
  if (!state_.contains("joystick_engaged"))
    state_["joystick_engaged"] = false;
  if (!state_.contains("joystick_speed"))
    state_["joystick_speed"] = "fast";
  if (!state_.contains("cmd_linear"))
    state_["cmd_linear"] = 0.0;
  if (!state_.contains("cmd_angular"))
    state_["cmd_angular"] = 0.0;
  if (!state_.contains("updated_at"))
    state_["updated_at"] = IdUtil::nowIso8601();
  if (!state_.contains("active_map_id"))
    state_["active_map_id"] = nullptr;
  if (!state_.contains("pose") || !state_["pose"].is_object())
    state_["pose"] = {{"x", 0.0}, {"y", 0.0}, {"yaw", 0.0}};
  saveUnlocked();
}

nlohmann::json RobotRuntime::buildStatusUnlocked() const
{
  const auto runner = mission_queue_.runnerStatus();
  const auto queue = mission_queue_.list();

  int pending = 0;
  if (queue.is_array())
  {
    for (const auto& item : queue)
    {
      if (item.value("status", "") == "pending")
        ++pending;
    }
  }

  const std::string motion = state_.value("motion", "paused");
  const std::string health = state_.value("health", "ok");
  const std::string runner_state = runner.value("state", "idle");

  std::string message = state_.value("message", kDefaultMessage);
  if (health == "ok")
  {
    if (!runner.value("message", "").empty())
      message = runner.value("message", "");
    else if (runner_state == "idle" && motion == "paused" && pending == 0)
      message = kDefaultMessage;
  }

  nlohmann::json body = {{"motion", motion},
                         {"health", health},
                         {"message", message},
                         {"error", state_.contains("error") ? state_["error"] : nullptr},
                         {"battery_percent", state_.value("battery_percent", 54)},
                         {"battery_charging", state_.value("battery_charging", false)},
                         {"joystick_engaged", state_.value("joystick_engaged", false)},
                         {"joystick_speed", state_.value("joystick_speed", "fast")},
                         {"cmd_linear", state_.value("cmd_linear", 0.0)},
                         {"cmd_angular", state_.value("cmd_angular", 0.0)},
                         {"active_map_id", state_.contains("active_map_id") ? state_["active_map_id"] : nullptr},
                         {"pose", state_.contains("pose") ? state_["pose"] : nlohmann::json::object({{"x", 0.0}, {"y", 0.0}, {"yaw", 0.0}})},
                         {"runner", runner},
                         {"queue_pending", pending},
                         {"updated_at", state_.value("updated_at", "")}};
  return body;
}

nlohmann::json RobotRuntime::status() const
{
  std::lock_guard<std::mutex> lock(mu_);
  return buildStatusUnlocked();
}

bool RobotRuntime::start(std::string& err)
{
  std::lock_guard<std::mutex> lock(mu_);
  if (state_.value("health", "ok") == "error")
  {
    err = "cannot start while robot is in error state — reset error first";
    return false;
  }

  state_["motion"] = "running";
  state_["message"] = "Robot running";
  state_["updated_at"] = IdUtil::nowIso8601();
  saveUnlocked();

  const std::string runner_state = mission_queue_.runnerStatus().value("state", "idle");
  if (runner_state == "paused")
    return mission_queue_.resume(err);
  return true;
}

bool RobotRuntime::pause(std::string& err)
{
  std::lock_guard<std::mutex> lock(mu_);
  state_["motion"] = "paused";
  state_["message"] = "Robot paused";
  state_["joystick_engaged"] = false;
  state_["cmd_linear"] = 0.0;
  state_["cmd_angular"] = 0.0;
  state_["updated_at"] = IdUtil::nowIso8601();
  saveUnlocked();

  const std::string runner_state = mission_queue_.runnerStatus().value("state", "idle");
  if (runner_state == "running" || runner_state == "paused")
    return mission_queue_.pause(err);
  return true;
}

bool RobotRuntime::resetError(std::string& err)
{
  (void)err;
  std::lock_guard<std::mutex> lock(mu_);
  state_["health"] = "ok";
  state_["error"] = nullptr;
  state_["message"] = kDefaultMessage;
  state_["updated_at"] = IdUtil::nowIso8601();
  saveUnlocked();
  return true;
}

bool RobotRuntime::setCmdVel(double linear, double angular, std::string& err)
{
  std::lock_guard<std::mutex> lock(mu_);
  if (state_.value("health", "ok") == "error")
  {
    err = "robot is in error state";
    return false;
  }
  if (!state_.value("joystick_engaged", false))
  {
    err = "joystick is not engaged";
    return false;
  }
  if (state_.value("motion", "paused") != "running")
  {
    err = "robot motion is paused — press START first";
    return false;
  }

  const std::string speed = state_.value("joystick_speed", "fast");
  double scale = 1.0;
  if (speed == "medium")
    scale = 0.55;
  else if (speed == "slow")
    scale = 0.25;

  linear = std::clamp(linear, -1.0, 1.0) * scale;
  angular = std::clamp(angular, -1.0, 1.0) * scale;

  state_["cmd_linear"] = linear;
  state_["cmd_angular"] = angular;
  state_["updated_at"] = IdUtil::nowIso8601();
  saveUnlocked();
  return true;
}

bool RobotRuntime::setJoystick(bool engaged, const std::string& speed, std::string& err)
{
  std::lock_guard<std::mutex> lock(mu_);
  if (engaged && state_.value("health", "ok") == "error")
  {
    err = "cannot engage joystick while robot is in error state";
    return false;
  }
  if (engaged && state_.value("motion", "paused") != "running")
  {
    err = "start the robot before engaging the joystick";
    return false;
  }

  state_["joystick_engaged"] = engaged;
  if (!speed.empty())
  {
    if (speed != "fast" && speed != "medium" && speed != "slow")
    {
      err = "speed must be fast, medium, or slow";
      return false;
    }
    state_["joystick_speed"] = speed;
  }
  if (!engaged)
  {
    state_["cmd_linear"] = 0.0;
    state_["cmd_angular"] = 0.0;
  }
  state_["updated_at"] = IdUtil::nowIso8601();
  saveUnlocked();
  return true;
}

bool RobotRuntime::setActiveMap(const std::string& map_id, std::string& err)
{
  (void)err;
  std::lock_guard<std::mutex> lock(mu_);
  state_["active_map_id"] = map_id;
  state_["updated_at"] = IdUtil::nowIso8601();
  saveUnlocked();
  return true;
}

void RobotRuntime::clearActiveMapIf(const std::string& map_id)
{
  std::lock_guard<std::mutex> lock(mu_);
  if (state_.value("active_map_id", "") == map_id)
  {
    state_["active_map_id"] = nullptr;
    state_["updated_at"] = IdUtil::nowIso8601();
    saveUnlocked();
  }
}

void RobotRuntime::tick()
{
  std::lock_guard<std::mutex> lock(mu_);
  const bool running = state_.value("motion", "paused") == "running";
  const bool joy = state_.value("joystick_engaged", false);
  double battery = state_.value("battery_percent", 54.0);

  if (running && (joy || std::abs(state_.value("cmd_linear", 0.0)) > 0.01))
    battery = std::max(0.0, battery - 0.02);
  else if (state_.value("battery_charging", false))
    battery = std::min(100.0, battery + 0.05);
  else if (!running)
    battery = std::min(100.0, battery + 0.005);

  state_["battery_percent"] = static_cast<int>(std::lround(battery));
  saveUnlocked();
}

}  // namespace lm