#include "map_server/map_server.h"


map_server::MapServer::MapServer(const std::string& fname, double res)
{
  ros::NodeHandle private_nh("~");
  this->initialize(private_nh, fname, res);
}

map_server::MapServer::MapServer(ros::NodeHandle nh, std::string name_sp, const std::string& fname, double res)
{
  nh_ = ros::NodeHandle(name_sp);
  this->initialize(nh, fname, res);
}

void map_server::MapServer::initialize(ros::NodeHandle nh, const std::string& fname, double res)
{
  std::string mapfname = "";
  double origin[3];
  int negate;
  double occ_th, free_th;
  MapMode mode = TRINARY;
  nh.param("frame_id", frame_id_, std::string("map"));
  nh.param("type", type_, 8);

  // When called this service returns a copy of the current map
  get_map_service_ = nh_.advertiseService("static_map", &map_server::MapServer::mapCallback, this);

  // Change the currently published map
  change_map_srv_ = nh_.advertiseService("change_map", &map_server::MapServer::changeMapCallback, this);

  // Latched publisher for metadata
  metadata_pub_ = nh_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);

  // Latched publisher for data
  map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
  if (type_ != 8) map_convert_ = nh_.advertise<nav_msgs::OccupancyGrid>("map_convert", 1, true);

  deprecated_ = (res != 0);
  if (!deprecated_)
  {
    if (!loadMapFromYaml(fname))
    {
      exit(-1);
    }
  }
  else
  {
    if (!loadMapFromParams(fname, res))
    {
      exit(-1);
    }
  }
}

bool map_server::MapServer::mapCallback(nav_msgs::GetMap::Request& req, nav_msgs::GetMap::Response& res)
{
  // request is empty; we ignore it

  // = operator is overloaded to make deep copy (tricky!)
  res = map_resp_;
  ROS_INFO("Sending map");

  return true;
}

bool map_server::MapServer::changeMapCallback(nav_msgs::LoadMap::Request& request,
  nav_msgs::LoadMap::Response& response)
{
  if (loadMapFromYaml(request.map_url))
  {
    response.result = response.RESULT_SUCCESS;
    ROS_INFO("Changed map to %s", request.map_url.c_str());
  }
  else
  {
    response.result = response.RESULT_UNDEFINED_FAILURE;
  }
  return true;
}


bool map_server::MapServer::loadMapFromValues(std::string map_file_name, double resolution,
  int negate, double occ_th, double free_th,
  double origin[3], MapMode mode)
{
  ROS_INFO("Loading map from images \"%s\"", map_file_name.c_str());
  try
  {
    if (type_ == 32)
      map_server::loadMapFromFile32Bit(&map_resp_, map_file_name.c_str(),
        resolution, negate, occ_th, free_th,
        origin, mode);
    else if (type_ == 16)
      map_server::loadMapFromFile16Bit(&map_resp_, map_file_name.c_str(),
        resolution, negate, occ_th, free_th,
        origin, mode);
    else
      map_server::loadMapFromFile(&map_resp_, map_file_name.c_str(),
        resolution, negate, occ_th, free_th,
        origin, mode);
  }
  catch (std::runtime_error& e)
  {
    ROS_ERROR("%s", e.what());
    return false;
  }

  // To make sure get a consistent time in simulation
  ros::Time::waitForValid();
  map_resp_.map.info.map_load_time = ros::Time::now();
  map_resp_.map.header.frame_id = frame_id_;
  map_resp_.map.header.stamp = ros::Time::now();
  ROS_INFO("Read a %d X %d map @ %.3lf m/cell",
    map_resp_.map.info.width,
    map_resp_.map.info.height,
    map_resp_.map.info.resolution);
  meta_data_message_ = map_resp_.map.info;

  // Publish latched topics
  metadata_pub_.publish(meta_data_message_);
  map_pub_.publish(map_resp_.map);

  if (type_ == 16)
  {
    double occ_th = 0.65;
    double free_th = 0.196;
    nav_msgs::OccupancyGrid convert;
    convert.header = map_resp_.map.header;
    convert.info = map_resp_.map.info;
    convert.info.width = map_resp_.map.info.width / 2;
    convert.data.resize(convert.info.width * convert.info.height);
    unsigned int color_sum, color;
    double color_avg;
    unsigned char value;
    int size_y = map_resp_.map.info.height;
    int size_x = map_resp_.map.info.width;

    for (int j = 0; j < map_resp_.map.info.height; j++)
    {
      for (int i = 0; i < map_resp_.map.info.width; i += 2)
      {
        unsigned char low = map_resp_.map.data[MAP_IDX(size_x, i, size_y - j - 1)];
        unsigned char high = map_resp_.map.data[MAP_IDX(size_x, i + 1, size_y - j - 1)];
        color = (int)((low & 0x00FF) | (high << 8));
        unsigned char red = (unsigned char)(color & 0x00FF0000) >> 16;
        unsigned char green = (unsigned char)(color & 0x0000FF00) >> 8;
        unsigned char blue = (unsigned char)(color & 0x000000FF);
        color_sum = red + green + blue;

        color_avg = color_sum / 2.0;

        double occ = (255 - color_avg) / 255.0;
        if (occ > occ_th)
          value = +100;
        else if (occ < free_th)
          value = 0;
        else
        {
          double ratio = (occ - free_th) / (occ_th - free_th);
          value = 1 + 98 * ratio;
        }
        convert.data[MAP_IDX(size_x / 2, i / 2, size_y - j - 1)] = value;
      }
    }
    map_convert_.publish(convert);
  }
  return true;
}


bool map_server::MapServer::loadMapFromParams(std::string map_file_name, double resolution)
{
  ros::NodeHandle private_nh("~");
  int negate;
  double occ_th;
  double free_th;
  double origin[3];
  private_nh.param("negate", negate, 0);
  private_nh.param("occupied_thresh", occ_th, 0.65);
  private_nh.param("free_thresh", free_th, 0.196);
  origin[0] = origin[1] = origin[2] = 0.0;
  return loadMapFromValues(map_file_name, resolution, negate, occ_th, free_th, origin, TRINARY);
}


bool map_server::MapServer::loadMapFromYaml(std::string path_to_yaml)
{
  std::string mapfname;
  MapMode mode;
  double res;
  int negate;
  double occ_th;
  double free_th;
  double origin[3];
  std::ifstream fin(path_to_yaml.c_str());
  if (fin.fail())
  {
    ROS_ERROR("Map_server could not open %s.", path_to_yaml.c_str());
    return false;
  }
#ifdef HAVE_YAMLCPP_GT_0_5_0
  // The document loading process changed in yaml-cpp 0.5.
  YAML::Node doc = YAML::Load(fin);
#else
  YAML::Parser parser(fin);
  YAML::Node doc;
  parser.GetNextDocument(doc);
#endif
  try
  {
    doc["resolution"] >> res;
  }
  catch (YAML::InvalidScalar&)
  {
    ROS_ERROR("The map does not contain a resolution tag or it is invalid.");
    return false;
  }
  try
  {
    doc["negate"] >> negate;
  }
  catch (YAML::InvalidScalar&)
  {
    ROS_ERROR("The map does not contain a negate tag or it is invalid.");
    return false;
  }
  try
  {
    doc["occupied_thresh"] >> occ_th;
  }
  catch (YAML::InvalidScalar&)
  {
    ROS_ERROR("The map does not contain an occupied_thresh tag or it is invalid.");
    return false;
  }
  try
  {
    doc["free_thresh"] >> free_th;
  }
  catch (YAML::InvalidScalar&)
  {
    ROS_ERROR("The map does not contain a free_thresh tag or it is invalid.");
    return false;
  }
  try
  {
    std::string modeS = "";
    doc["mode"] >> modeS;

    if (modeS == "trinary")
      mode = TRINARY;
    else if (modeS == "scale")
      mode = SCALE;
    else if (modeS == "raw")
      mode = RAW;
    else
    {
      ROS_ERROR("Invalid mode tag \"%s\".", modeS.c_str());
      return false;
    }
  }
  catch (YAML::Exception&)
  {
    ROS_DEBUG("The map does not contain a mode tag or it is invalid... assuming Trinary");
    mode = TRINARY;
  }
  try
  {
    doc["origin"][0] >> origin[0];
    doc["origin"][1] >> origin[1];
    doc["origin"][2] >> origin[2];
  }
  catch (YAML::InvalidScalar&)
  {
    ROS_ERROR("The map does not contain an origin tag or it is invalid.");
    return false;
  }
  try
  {
    doc["image"] >> mapfname;
    // TODO: make this path-handling more robust
    if (mapfname.size() == 0)
    {
      ROS_ERROR("The image tag cannot be an empty string.");
      return false;
    }

    boost::filesystem::path mapfpath(mapfname);
    if (!mapfpath.is_absolute())
    {
      boost::filesystem::path dir(path_to_yaml);
      dir = dir.parent_path();
      mapfpath = dir / mapfpath;
      mapfname = mapfpath.string();
    }
  }
  catch (YAML::InvalidScalar&)
  {
    ROS_ERROR("The map does not contain an image tag or it is invalid.");
    return false;
  }
  return loadMapFromValues(mapfname, res, negate, occ_th, free_th, origin, mode);
}