#include <robot/init.h>
#include <robot/console.h>
#include <csignal>
#include <atomic>
#include <mutex>

namespace robot
{

namespace detail
{
  // Internal state for shutdown management
  std::atomic<bool> g_shutdown_requested(false);
  std::atomic<bool> g_shutdown_complete(false);
  std::atomic<bool> g_initialized(false);
  std::mutex g_shutdown_mutex;
  bool g_sigint_handler_installed = false;
  
  // Original SIGINT handler (if any)
  void (*g_original_sigint_handler)(int) = nullptr;
}

void detail::sigintHandler(int sig)
{
  (void)sig; // Suppress unused parameter warning
  
  // Call shutdown
  robot::shutdown();
  
  // If there was an original handler, call it too
  if (detail::g_original_sigint_handler && 
      detail::g_original_sigint_handler != SIG_DFL && 
      detail::g_original_sigint_handler != SIG_IGN)
  {
    detail::g_original_sigint_handler(sig);
  }
}

bool ok()
{
  // Return false if shutdown is complete
  return !detail::g_shutdown_complete.load(std::memory_order_acquire);
}

bool isShuttingDown()
{
  // Return true if shutdown has been requested
  return detail::g_shutdown_requested.load(std::memory_order_acquire);
}

void shutdown()
{
  std::lock_guard<std::mutex> lock(detail::g_shutdown_mutex);
  
  // If already shutting down, do nothing
  if (detail::g_shutdown_requested.load(std::memory_order_acquire))
  {
    return;
  }
  
  // Mark shutdown as requested
  detail::g_shutdown_requested.store(true, std::memory_order_release);
  
  // Log shutdown
  robot::log_info("Shutting down robot system...");
  
  // TODO: Add cleanup of resources here if needed
  // For example:
  // - Close all subscriptions/publications
  // - Stop all timers
  // - Cleanup threads
  
  // Mark shutdown as complete
  // In a more complex system, you might want to wait for resources to cleanup
  detail::g_shutdown_complete.store(true, std::memory_order_release);
  
  robot::log_info("Robot system shutdown complete");
}

void init(int& argc, char** argv, const std::string& node_name, bool install_sigint_handler)
{
  (void)argc;  // Suppress unused parameter warning
  (void)argv;  // Suppress unused parameter warning
  
  std::lock_guard<std::mutex> lock(detail::g_shutdown_mutex);
  
  // If already initialized, do nothing
  if (detail::g_initialized.load(std::memory_order_acquire))
  {
    robot::log_warning("robot::init() called multiple times, ignoring");
    return;
  }
  
  // Reset shutdown state
  detail::g_shutdown_requested.store(false, std::memory_order_release);
  detail::g_shutdown_complete.store(false, std::memory_order_release);
  
  // Install SIGINT handler if requested
  if (install_sigint_handler)
  {
    // Save original handler
    detail::g_original_sigint_handler = std::signal(SIGINT, detail::sigintHandler);
    detail::g_sigint_handler_installed = true;
    
    if (!node_name.empty())
    {
      robot::log_info("Initialized robot system for node: %s", node_name.c_str());
    }
    else
    {
      robot::log_info("Initialized robot system");
    }
  }
  else
  {
    if (!node_name.empty())
    {
      robot::log_info("Initialized robot system for node: %s (no SIGINT handler)", node_name.c_str());
    }
    else
    {
      robot::log_info("Initialized robot system (no SIGINT handler)");
    }
  }
  
  // Mark as initialized
  detail::g_initialized.store(true, std::memory_order_release);
}

void init(const std::string& node_name, bool install_sigint_handler)
{
  // Dummy argc/argv for the other init() function
  int dummy_argc = 0;
  char** dummy_argv = nullptr;
  init(dummy_argc, dummy_argv, node_name, install_sigint_handler);
}

} // namespace robot

