#include <boost/filesystem.hpp>
#include <boost/exception/exception.hpp>
#include <boost/system/error_code.hpp>
#include <ros/package.h>
#include <fstream>
#include "loc_base/loc_base_util.h"

std::string loc_base::LocBaseUtil::filepathFromPackage(const std::string& package, const std::string& filename)
{
  std::string filepath = filename;

  if (!package.empty())
  {
    // Get the package path using ROS package utility
    std::string pkg_path = ros::package::getPath(package);

    // Log package path (info level, could be changed to ROS_DEBUG for less verbosity)
    // ROS_INFO_STREAM("Package path: " << pkg_path);

    if (!pkg_path.empty())
    {
      // Construct the full file path using Boost Filesystem (or std::filesystem)
      filepath = (boost::filesystem::path(pkg_path) / filename).make_preferred().native();
      // For C++17 and later, you can use the following line instead:
      // filepath = (std::filesystem::path(pkg_path) / filename).string();

      // Check if the file exists
      if (!boost::filesystem::exists(filepath))  // Or std::filesystem::exists(filepath) for C++17+
      {
        // If file does not exist, throw an exception
        std::string error_message = "File does not exist at: " + filepath;
        // ROS_ERROR_STREAM(error_message);
        throw std::runtime_error(error_message);
      }
    }
    else
    {
      // If package path is empty, throw an exception
      std::string error_message = "Failed to get path for package '" + package + "'.";
      // ROS_ERROR_STREAM(error_message);
      throw std::runtime_error(error_message);
    }
  }

  return filepath;
}



bool loc_base::LocBaseUtil::loadActivateMapFileName(const std::string& filename, std::string& activated_map_filename)
{
  // Check if saved path file is existed
  std::ifstream inputFile;
  inputFile.open(filename);
  std::stringstream buffer;
  if (inputFile)
  {
    buffer << inputFile.rdbuf();
    inputFile.close();
    if (buffer.str().empty())
      return false;
  }
  else return false;
  activated_map_filename = buffer.str();

  return true;
}

bool loc_base::LocBaseUtil::saveActivateMapFileName(const std::string& filename, const std::string& activated_map_filename)
{
  bool result = false;
  // Check if saved path file is existed
  std::ofstream outputFile(filename, std::ios::out);
  if (outputFile.is_open()) {
    outputFile << activated_map_filename;
    outputFile.close();
    result = true;
  }
  else
    result = false;
  return result;
}

bool loc_base::LocBaseUtil::convertPgmToPng(const std::string& pgmFileName, const std::string& pngFileName)
{
  std::string cmd;
  int retVal;
  cmd = "convert " + pgmFileName + " " + pngFileName;
  retVal = system(cmd.c_str());
  if (retVal != 0)
    return false;

  return true;
}

bool loc_base::LocBaseUtil::writeGridMap(const std::string& filename)
{
  std::string pgmFileName = filename + ".pgm";
  std::string pngFileName = filename + ".png";
  std::string yamlFileName = filename + ".yaml";
  // bool is_SaveStateAsFiles = SaveStateAsFiles(filename); // cái này tham khảo https://github.com/ros-planning/navigation/blob/ac41d2480c4cf1602daf39a6e9629142731d92b0/map_server/src/map_saver.cpp#L114
  // if(is_SaveStateAsFiles)
  // {
  //   if(convertPgmToPng(pgmFileName, pngFileName)) 
  //     return true;
  // }
  return false;
}

bool loc_base::LocBaseUtil::checkIfMapFilesExist(const std::string& filename)
{

  std::string SavedGridMapFileName = filename + ".png";
  std::string SavedMapInfoFileName = filename + ".yaml";

  std::ifstream SavedGridMapFile(SavedGridMapFileName);
  std::ifstream SavedMapInfoFile(SavedMapInfoFileName);
  if (SavedGridMapFile.good() && SavedMapInfoFile.good())
  {
    ROS_INFO("map file: %s is found", filename.c_str());
    return true;
  }
  else ROS_WARN("map file: %s is not found", filename.c_str());
  return false;
}

bool loc_base::LocBaseUtil::replaceMapDirectory(const std::string& mapDirectory)
{
  int retVal_1 = -1;
  int retVal_2 = -1;
  std::string removeCmd = "rm -rf " + mapDirectory;
  retVal_1 = system(removeCmd.c_str());
  if (retVal_1 == 0)
  {
    std::string createCmd = "mkdir " + mapDirectory;
    retVal_2 = system(createCmd.c_str());
  }
  if (retVal_2 == 0)
  {
    return true;
  }
  else
  {
    return false;
  }
  return false;
}

std::stringstream  loc_base::LocBaseUtil::listMapFiles(const std::string& path)
{
  std::stringstream directories;
  try
  {
    for (const auto& entry : boost::filesystem::directory_iterator(path)) {
      if(entry.path().extension() == ".txt")
        continue;
      directories << entry.path().filename() << std::endl;
    }
  }
  catch (const boost::system::system_error& e) {
    // Catching boost::system::system_error (which inherits from boost::exception)
    ROS_ERROR_STREAM("Error reading directory: " << e.what());
  }
  catch (const boost::exception& e) {
    // Catching general boost exceptions
    ROS_ERROR_STREAM("Boost exception: List Files failed");
  }
  return directories;
}