#ifndef ORIGINCAR_BASE__COMMAND_GATE_HPP_
#define ORIGINCAR_BASE__COMMAND_GATE_HPP_

#include <chrono>
#include <mutex>

namespace origincar_base_core
{

struct Command
{
  double linear_x{0.0};
  double linear_y{0.0};
  double angular_z{0.0};
  double steering_angle{0.0};
  bool ackermann{false};
};

inline bool operator==(const Command & lhs, const Command & rhs)
{
  return lhs.linear_x == rhs.linear_x &&
         lhs.linear_y == rhs.linear_y &&
         lhs.angular_z == rhs.angular_z &&
         lhs.steering_angle == rhs.steering_angle &&
         lhs.ackermann == rhs.ackermann;
}

class CommandGate
{
public:
  explicit CommandGate(std::chrono::milliseconds timeout)
  : timeout_(timeout)
  {
  }

  void update(const Command & command, std::chrono::steady_clock::time_point now)
  {
    std::lock_guard<std::mutex> lock(mutex_);
    latest_command_ = command;
    last_command_time_ = now;
    has_command_ = true;
  }

  Command commandAt(std::chrono::steady_clock::time_point now) const
  {
    std::lock_guard<std::mutex> lock(mutex_);
    if (!has_command_ || now - last_command_time_ >= timeout_) {
      return {};
    }
    return latest_command_;
  }

private:
  const std::chrono::milliseconds timeout_;
  mutable std::mutex mutex_;
  Command latest_command_{};
  std::chrono::steady_clock::time_point last_command_time_{};
  bool has_command_{false};
};

}  // namespace origincar_base_core

#endif  // ORIGINCAR_BASE__COMMAND_GATE_HPP_
