/*********************************************************************
 * WallTime - Standalone Wall-clock Time Library
 * 
 * Header-only library for wall-clock time operations.
 * This is a simplified, standalone version of WallTime that can be
 * used independently or as a convenience wrapper.
 * 
 * Features:
 * - Always uses real wall-clock time (not affected by simulated time)
 * - No initialization required
 * - Thread-safe
 * - Cross-platform (Linux, Windows, macOS)
 * - Nanosecond precision
 * 
 * Usage:
 *   #include <robot/walltime.h>
 *   
 *   robot::WallTime now = robot::WallTime::now();
 *   robot::WallDuration elapsed = robot::WallTime::now() - now;
 *********************************************************************/

#ifndef ROBOT_WALLTIME_H
#define ROBOT_WALLTIME_H

#include <chrono>
#include <cstdint>
#include <iostream>
#include <iomanip>
#include <limits>
#include <cmath>
#include <thread>
#include <stdexcept>

namespace robot
{

/**
 * \brief WallDuration - Represents a time interval using wall-clock time
 */
class WallDuration
{
public:
  int32_t sec;   // Seconds (can be negative)
  int32_t nsec;  // Nanoseconds (0-999999999)

  // Constructors
  WallDuration() : sec(0), nsec(0) {}
  
  WallDuration(int32_t _sec, int32_t _nsec) : sec(_sec), nsec(_nsec)
  {
    normalize();
  }
  
  explicit WallDuration(double t)
  {
    fromSec(t);
  }

  // Normalize nanoseconds to [0, 999999999] range
  void normalize()
  {
    int64_t sec64 = sec;
    int64_t nsec64 = nsec;
    
    // Handle negative nanoseconds
    while (nsec64 < 0)
    {
      nsec64 += 1000000000LL;
      sec64 -= 1;
    }
    
    // Handle overflow nanoseconds
    while (nsec64 >= 1000000000LL)
    {
      nsec64 -= 1000000000LL;
      sec64 += 1;
    }
    
    // Check bounds
    if (sec64 > std::numeric_limits<int32_t>::max() || 
        sec64 < std::numeric_limits<int32_t>::min())
    {
      throw std::runtime_error("WallDuration out of range");
    }
    
    sec = static_cast<int32_t>(sec64);
    nsec = static_cast<int32_t>(nsec64);
  }

  // Convert to seconds (double)
  double toSec() const
  {
    return static_cast<double>(sec) + 1e-9 * static_cast<double>(nsec);
  }

  // Convert to nanoseconds
  int64_t toNSec() const
  {
    return static_cast<int64_t>(sec) * 1000000000LL + static_cast<int64_t>(nsec);
  }

  // Initialize from seconds
  WallDuration& fromSec(double t)
  {
    sec = static_cast<int32_t>(std::floor(t));
    nsec = static_cast<int32_t>((t - sec) * 1e9);
    normalize();
    return *this;
  }

  // Initialize from nanoseconds
  WallDuration& fromNSec(int64_t t)
  {
    sec = static_cast<int32_t>(t / 1000000000LL);
    nsec = static_cast<int32_t>(t % 1000000000LL);
    normalize();
    return *this;
  }

  // Arithmetic operations
  WallDuration operator+(const WallDuration& rhs) const
  {
    return WallDuration(sec + rhs.sec, nsec + rhs.nsec);
  }

  WallDuration operator-(const WallDuration& rhs) const
  {
    return WallDuration(sec - rhs.sec, nsec - rhs.nsec);
  }

  WallDuration operator-() const
  {
    return WallDuration(-sec, -nsec);
  }

  WallDuration operator*(double scale) const
  {
    return WallDuration(toSec() * scale);
  }

  WallDuration& operator+=(const WallDuration& rhs)
  {
    sec += rhs.sec;
    nsec += rhs.nsec;
    normalize();
    return *this;
  }

  WallDuration& operator-=(const WallDuration& rhs)
  {
    sec -= rhs.sec;
    nsec -= rhs.nsec;
    normalize();
    return *this;
  }

  // Comparison operators
  bool operator==(const WallDuration& rhs) const
  {
    return sec == rhs.sec && nsec == rhs.nsec;
  }

  bool operator!=(const WallDuration& rhs) const
  {
    return !(*this == rhs);
  }

  bool operator<(const WallDuration& rhs) const
  {
    if (sec < rhs.sec) return true;
    if (sec > rhs.sec) return false;
    return nsec < rhs.nsec;
  }

  bool operator>(const WallDuration& rhs) const
  {
    return rhs < *this;
  }

  bool operator<=(const WallDuration& rhs) const
  {
    return !(rhs < *this);
  }

  bool operator>=(const WallDuration& rhs) const
  {
    return !(*this < rhs);
  }

  // Sleep for this duration
  bool sleep() const
  {
    if (sec < 0) return false;
    
    using namespace std::chrono;
    std::chrono::nanoseconds ns(toNSec());
    std::this_thread::sleep_for(ns);
    return true;
  }

  // Check if zero
  bool isZero() const
  {
    return sec == 0 && nsec == 0;
  }

  // Constants
  static const WallDuration ZERO;
  static const WallDuration MAX;
  static const WallDuration MIN;
};

/**
 * \brief WallTime - Represents a point in time using wall-clock time
 */
class WallTime
{
public:
  uint32_t sec;   // Seconds since epoch
  uint32_t nsec;  // Nanoseconds (0-999999999)

  // Constructors
  WallTime() : sec(0), nsec(0) {}
  
  WallTime(uint32_t _sec, uint32_t _nsec) : sec(_sec), nsec(_nsec)
  {
    normalize();
  }
  
  explicit WallTime(double t)
  {
    fromSec(t);
  }

  // Normalize nanoseconds to [0, 999999999] range
  void normalize()
  {
    uint64_t sec64 = sec;
    uint64_t nsec64 = nsec;
    
    // Handle overflow nanoseconds
    uint64_t sec_part = nsec64 / 1000000000ULL;
    nsec64 = nsec64 % 1000000000ULL;
    sec64 += sec_part;
    
    // Check bounds
    if (sec64 > std::numeric_limits<uint32_t>::max())
    {
      throw std::runtime_error("WallTime out of range");
    }
    
    sec = static_cast<uint32_t>(sec64);
    nsec = static_cast<uint32_t>(nsec64);
  }

  // Get current wall-clock time
  static WallTime now()
  {
    using namespace std::chrono;
    auto now_time = system_clock::now();
    auto duration = now_time.time_since_epoch();
    auto nanoseconds_count = duration_cast<std::chrono::nanoseconds>(duration).count();
    
    WallTime t;
    uint64_t sec64 = nanoseconds_count / 1000000000ULL;
    uint64_t nsec64 = nanoseconds_count % 1000000000ULL;
    
    if (sec64 > std::numeric_limits<uint32_t>::max())
    {
      throw std::runtime_error("WallTime::now() - time out of range");
    }
    
    t.sec = static_cast<uint32_t>(sec64);
    t.nsec = static_cast<uint32_t>(nsec64);
    
    return t;
  }

  // Convert to seconds (double)
  double toSec() const
  {
    return static_cast<double>(sec) + 1e-9 * static_cast<double>(nsec);
  }

  // Convert to nanoseconds
  uint64_t toNSec() const
  {
    return static_cast<uint64_t>(sec) * 1000000000ULL + static_cast<uint64_t>(nsec);
  }

  // Initialize from seconds
  WallTime& fromSec(double t)
  {
    sec = static_cast<uint32_t>(std::floor(t));
    nsec = static_cast<uint32_t>((t - sec) * 1e9);
    normalize();
    return *this;
  }

  // Initialize from nanoseconds
  WallTime& fromNSec(uint64_t t)
  {
    sec = static_cast<uint32_t>(t / 1000000000ULL);
    nsec = static_cast<uint32_t>(t % 1000000000ULL);
    return *this;
  }

  // Arithmetic operations with WallDuration
  WallTime operator+(const WallDuration& d) const
  {
    int64_t total_sec = static_cast<int64_t>(sec) + d.sec;
    int64_t total_nsec = static_cast<int64_t>(nsec) + d.nsec;
    
    if (total_sec < 0)
    {
      throw std::runtime_error("WallTime::operator+ - result would be negative");
    }
    
    WallTime result;
    result.sec = static_cast<uint32_t>(total_sec);
    result.nsec = static_cast<uint32_t>(total_nsec);
    result.normalize();
    return result;
  }

  WallTime operator-(const WallDuration& d) const
  {
    int64_t total_sec = static_cast<int64_t>(sec) - d.sec;
    int64_t total_nsec = static_cast<int64_t>(nsec) - d.nsec;
    
    if (total_sec < 0)
    {
      throw std::runtime_error("WallTime::operator- - result would be negative");
    }
    
    WallTime result;
    result.sec = static_cast<uint32_t>(total_sec);
    result.nsec = static_cast<uint32_t>(total_nsec);
    result.normalize();
    return result;
  }

  WallDuration operator-(const WallTime& rhs) const
  {
    int64_t sec_diff = static_cast<int64_t>(sec) - static_cast<int64_t>(rhs.sec);
    int64_t nsec_diff = static_cast<int64_t>(nsec) - static_cast<int64_t>(rhs.nsec);
    
    return WallDuration(static_cast<int32_t>(sec_diff), static_cast<int32_t>(nsec_diff));
  }

  WallTime& operator+=(const WallDuration& d)
  {
    *this = *this + d;
    return *this;
  }

  WallTime& operator-=(const WallDuration& d)
  {
    *this = *this - d;
    return *this;
  }

  // Comparison operators
  bool operator==(const WallTime& rhs) const
  {
    return sec == rhs.sec && nsec == rhs.nsec;
  }

  bool operator!=(const WallTime& rhs) const
  {
    return !(*this == rhs);
  }

  bool operator<(const WallTime& rhs) const
  {
    if (sec < rhs.sec) return true;
    if (sec > rhs.sec) return false;
    return nsec < rhs.nsec;
  }

  bool operator>(const WallTime& rhs) const
  {
    return rhs < *this;
  }

  bool operator<=(const WallTime& rhs) const
  {
    return !(rhs < *this);
  }

  bool operator>=(const WallTime& rhs) const
  {
    return !(*this < rhs);
  }

  // Sleep until this time
  static bool sleepUntil(const WallTime& end)
  {
    WallTime now_time = now();
    if (end <= now_time)
    {
      return true;  // Already past the target time
    }
    
    WallDuration remaining = end - now_time;
    return remaining.sleep();
  }

  // Check if zero
  bool isZero() const
  {
    return sec == 0 && nsec == 0;
  }

  // Check if this is system time (always true for WallTime)
  static bool isSystemTime()
  {
    return true;
  }

  // Constants
  static const WallTime ZERO;
  static const WallTime MAX;
  static const WallTime MIN;
};

// Stream operators
inline std::ostream& operator<<(std::ostream& os, const WallTime& rhs)
{
  auto flags = os.flags();
  auto fillc = os.fill();
  auto width = os.width();
  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
  os.flags(flags);
  os.fill(fillc);
  os.width(width);
  return os;
}

inline std::ostream& operator<<(std::ostream& os, const WallDuration& rhs)
{
  auto flags = os.flags();
  auto fillc = os.fill();
  auto width = os.width();
  if (rhs.sec >= 0 || rhs.nsec == 0)
  {
    os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
  }
  else
  {
    os << (rhs.sec == -1 ? "-" : "") << (rhs.sec + 1) << "." 
       << std::setw(9) << std::setfill('0') << (1000000000 - rhs.nsec);
  }
  os.flags(flags);
  os.fill(fillc);
  os.width(width);
  return os;
}

// Constants definitions (inline to avoid multiple definition)
inline const WallDuration WallDuration::ZERO(0, 0);
inline const WallDuration WallDuration::MAX(
  std::numeric_limits<int32_t>::max(), 999999999);
inline const WallDuration WallDuration::MIN(
  std::numeric_limits<int32_t>::min(), 0);

inline const WallTime WallTime::ZERO(0, 0);
inline const WallTime WallTime::MAX(
  std::numeric_limits<uint32_t>::max(), 999999999);
inline const WallTime WallTime::MIN(0, 1);

} // namespace robot

#endif // ROBOT_WALLTIME_H