#include "util/ros_map_yaml.hpp"

#include "util/string_util.hpp"

#include <sstream>

namespace lm {

namespace {

std::string stripComment(const std::string& line)
{
  const auto pos = line.find('#');
  if (pos == std::string::npos)
    return line;
  return line.substr(0, pos);
}

std::string trim(const std::string& s)
{
  return StringUtil::trimCopy(s);
}

bool parseOriginArray(const std::string& value, RosMapYaml& out)
{
  const auto start = value.find('[');
  const auto end = value.find(']');
  if (start == std::string::npos || end == std::string::npos || end <= start)
    return false;

  std::string inner = value.substr(start + 1, end - start - 1);
  for (char& c : inner)
  {
    if (c == ',')
      c = ' ';
  }
  std::istringstream iss(inner);
  double x = 0, y = 0, yaw = 0;
  if (!(iss >> x >> y))
    return false;
  iss >> yaw;
  out.origin_x = x;
  out.origin_y = y;
  out.origin_yaw = yaw;
  return true;
}

double parseDouble(const std::string& value, bool& ok)
{
  ok = false;
  try
  {
    size_t idx = 0;
    const double v = std::stod(value, &idx);
    if (idx > 0)
      ok = true;
    return v;
  }
  catch (...)
  {
    return 0.0;
  }
}

int parseInt(const std::string& value, bool& ok)
{
  ok = false;
  try
  {
    size_t idx = 0;
    const int v = std::stoi(value, &idx);
    if (idx > 0)
      ok = true;
    return v;
  }
  catch (...)
  {
    return 0;
  }
}

}  // namespace

std::optional<RosMapYaml> RosMapYamlParser::parse(const std::string& yaml_text, std::string& err)
{
  RosMapYaml out;
  std::istringstream stream(yaml_text);
  std::string line;
  bool has_resolution = false;

  while (std::getline(stream, line))
  {
    line = trim(stripComment(line));
    if (line.empty())
      continue;

    const auto colon = line.find(':');
    if (colon == std::string::npos)
      continue;

    const std::string key = trim(line.substr(0, colon));
    const std::string value = trim(line.substr(colon + 1));
    if (value.empty() && key != "image")
      continue;

    if (key == "image")
    {
      out.image = value;
    }
    else if (key == "resolution")
    {
      bool ok = false;
      out.resolution = parseDouble(value, ok);
      if (!ok || out.resolution <= 0.0)
      {
        err = "invalid resolution in yaml";
        return std::nullopt;
      }
      has_resolution = true;
    }
    else if (key == "origin")
    {
      if (!parseOriginArray(value, out))
      {
        err = "invalid origin in yaml";
        return std::nullopt;
      }
    }
    else if (key == "negate")
    {
      bool ok = false;
      out.negate = parseInt(value, ok);
      if (!ok)
        out.negate = 0;
    }
    else if (key == "occupied_thresh")
    {
      bool ok = false;
      out.occupied_thresh = parseDouble(value, ok);
      if (!ok)
        out.occupied_thresh = 0.65;
    }
    else if (key == "free_thresh")
    {
      bool ok = false;
      out.free_thresh = parseDouble(value, ok);
      if (!ok)
        out.free_thresh = 0.196;
    }
  }

  if (out.image.empty())
  {
    err = "yaml missing image field";
    return std::nullopt;
  }
  if (!has_resolution)
  {
    err = "yaml missing resolution field";
    return std::nullopt;
  }

  return out;
}

}  // namespace lm
