#include "validation/sensor_validator.hpp"

#include "util/string_util.hpp"

namespace lm {

std::optional<size_t> SensorValidator::findLidarIndex(const nlohmann::json& state, const std::string& id)
{
  if (!state.contains("lidars") || !state["lidars"].is_array())
    return std::nullopt;
  const auto& lidars = state["lidars"];
  for (size_t i = 0; i < lidars.size(); i++)
  {
    const auto& l = lidars[i];
    if (l.is_object() && l.contains("id") && l["id"].is_string() && l["id"].get<std::string>() == id)
      return i;
  }
  return std::nullopt;
}

std::optional<size_t> SensorValidator::findImuIndex(const nlohmann::json& state, const std::string& id)
{
  if (!state.contains("imus") || !state["imus"].is_array())
    return std::nullopt;
  const auto& imus = state["imus"];
  for (size_t i = 0; i < imus.size(); i++)
  {
    const auto& im = imus[i];
    if (im.is_object() && im.contains("id") && im["id"].is_string() && im["id"].get<std::string>() == id)
      return i;
  }
  return std::nullopt;
}

bool SensorValidator::validateLidarPayload(const nlohmann::json& payload, std::string& err)
{
  if (!payload.is_object())
  {
    err = "payload must be a JSON object";
    return false;
  }
  if (!payload.contains("name") || !payload["name"].is_string() || payload["name"].get<std::string>().empty())
  {
    err = "name is required";
    return false;
  }
  if (!payload.contains("ip") || !payload["ip"].is_string() || payload["ip"].get<std::string>().empty())
  {
    err = "ip is required";
    return false;
  }
  if (!payload.contains("port") || !payload["port"].is_number_integer())
  {
    err = "port must be an integer";
    return false;
  }
  const int port = payload["port"].get<int>();
  if (port < 1 || port > 65535)
  {
    err = "port must be in range 1..65535";
    return false;
  }
  return true;
}

bool SensorValidator::lidarTripletExists(const nlohmann::json& state,
                                 const std::string& name,
                                 const std::string& ip,
                                 int port,
                                 const std::string* exclude_id)
{
  if (!state.contains("lidars") || !state["lidars"].is_array())
    return false;
  const std::string n = StringUtil::trimCopy(name);
  const std::string i = StringUtil::trimCopy(ip);
  for (const auto& l : state["lidars"])
  {
    if (!l.is_object())
      continue;
    if (exclude_id && l.contains("id") && l["id"].get<std::string>() == *exclude_id)
      continue;
    if (!l.contains("name") || !l.contains("ip") || !l.contains("port"))
      continue;
    if (StringUtil::trimCopy(l["name"].get<std::string>()) == n && StringUtil::trimCopy(l["ip"].get<std::string>()) == i &&
        l["port"].get<int>() == port)
      return true;
  }
  return false;
}

bool SensorValidator::validateImuPayload(const nlohmann::json& payload, std::string& err)
{
  if (!payload.is_object())
  {
    err = "payload must be a JSON object";
    return false;
  }
  if (!payload.contains("name") || !payload["name"].is_string() || payload["name"].get<std::string>().empty())
  {
    err = "name is required";
    return false;
  }
  if (!payload.contains("frame_id") || !payload["frame_id"].is_string() ||
      payload["frame_id"].get<std::string>().empty())
  {
    err = "frame_id is required";
    return false;
  }
  if (!payload.contains("topic") || !payload["topic"].is_string() || payload["topic"].get<std::string>().empty())
  {
    err = "topic is required";
    return false;
  }
  if (payload.contains("source") && payload["source"].is_string())
  {
    const std::string src = payload["source"].get<std::string>();
    if (src != "external" && src != "lidar_builtin" && src != "onboard")
    {
      err = "source must be external, lidar_builtin, or onboard";
      return false;
    }
  }
  if (payload.contains("rate_hz") && !payload["rate_hz"].is_number())
  {
    err = "rate_hz must be a number";
    return false;
  }
  return true;
}

bool SensorValidator::imuFrameExists(const nlohmann::json& state,
                             const std::string& frame_id,
                             const std::string* exclude_id)
{
  if (!state.contains("imus") || !state["imus"].is_array())
    return false;
  const std::string f = StringUtil::trimCopy(frame_id);
  for (const auto& im : state["imus"])
  {
    if (!im.is_object())
      continue;
    if (exclude_id && im.contains("id") && im["id"].get<std::string>() == *exclude_id)
      continue;
    if (!im.contains("frame_id"))
      continue;
    if (StringUtil::trimCopy(im["frame_id"].get<std::string>()) == f)
      return true;
  }
  return false;
}
}  // namespace lm
