#include <chrono>

#include <gtest/gtest.h>

#include "origincar_base/command_gate.hpp"

using origincar_base_core::Command;
using origincar_base_core::CommandGate;

TEST(CommandGateTest, ReturnsZeroBeforeFirstCommand)
{
  const auto t0 = std::chrono::steady_clock::time_point{};
  CommandGate gate(std::chrono::milliseconds(150));

  EXPECT_EQ(gate.commandAt(t0), (Command{}));
}

TEST(CommandGateTest, KeepsLatestCommandUntilWatchdogExpires)
{
  const auto t0 = std::chrono::steady_clock::time_point{};
  CommandGate gate(std::chrono::milliseconds(150));
  const Command command{0.25, 0.0, 0.4};

  gate.update(command, t0);

  EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(149)), command);
  EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(150)), (Command{}));
}

TEST(CommandGateTest, NewCommandRefreshesWatchdog)
{
  const auto t0 = std::chrono::steady_clock::time_point{};
  CommandGate gate(std::chrono::milliseconds(150));
  const Command first{0.1, 0.0, 0.1};
  const Command second{0.3, 0.0, -0.2};

  gate.update(first, t0);
  gate.update(second, t0 + std::chrono::milliseconds(100));

  EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(249)), second);
  EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(250)), (Command{}));
}
