test_primary_client.cpp

// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2025 Universal Robots A/S
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright
//      notice, this list of conditions and the following disclaimer.
//
//    * Redistributions in binary form must reproduce the above copyright
//      notice, this list of conditions and the following disclaimer in the
//      documentation and/or other materials provided with the distribution.
//
//    * Neither the name of the {copyright_holder} nor the names of its
//      contributors may be used to endorse or promote products derived from
//      this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
// -- END LICENSE BLOCK ------------------------------------------------

#include <gtest/gtest.h>

#include <ur_client_library/primary/primary_client.h>
#include <ur_client_library/ur/calibration_checker.h>
#include <chrono>
#include <memory>
#include <queue>
#include <stdexcept>
#include <thread>
#include "fake_primary_server.h"
#include "ur_client_library/exceptions.h"
#include "ur_client_library/helpers.h"

using namespace urcl;

std::string g_ROBOT_IP = "192.168.56.101";

class RobotMessageConsumer : public comm::IConsumer<primary_interface::PrimaryPackage>
{
public:
  RobotMessageConsumer() = default;
  virtual ~RobotMessageConsumer() = default;

  virtual bool consume(std::shared_ptr<primary_interface::PrimaryPackage> product)
  {
    auto message_ptr = std::dynamic_pointer_cast<primary_interface::RobotMessage>(product);

    if (message_ptr != nullptr)
    {
      std::lock_guard<std::mutex> lock(data_lock_);
      package_queue_.push(message_ptr);
      URCL_LOG_INFO("%s", message_ptr->toString().c_str());
      data_cv_.notify_one();
      return true;
    }
    return true;
  }

  std::shared_ptr<primary_interface::RobotMessage>
  getOrWaitForMessage(const std::chrono::milliseconds timeout = std::chrono::milliseconds(1000))
  {
    std::unique_lock<std::mutex> lock(data_lock_);
    if (!package_queue_.empty() || data_cv_.wait_for(lock, timeout) == std::cv_status::no_timeout)
    {
      auto data = package_queue_.front();
      package_queue_.pop();
      return data;
    }
    throw TimeoutException("Did not receive RobotMessage in time", timeout);
  }

private:
  std::queue<std::shared_ptr<primary_interface::RobotMessage>> package_queue_;
  std::condition_variable data_cv_;
  std::mutex data_lock_;
};

class PrimaryClientTest : public ::testing::Test
{
protected:
  void SetUp() override
  {
    client_ = std::make_unique<primary_interface::PrimaryClient>(g_ROBOT_IP, notifier_);
  }

  std::unique_ptr<primary_interface::PrimaryClient> client_;
  comm::INotifier notifier_;
};

class PrimaryClientFakeTest : public ::testing::Test
{
protected:
  void SetUp() override
  {
    server_ = std::make_unique<FakePrimaryServer>(30001);
    client_ = std::make_unique<primary_interface::PrimaryClient>("127.0.0.1", notifier_);
    EXPECT_NO_THROW(client_->start());
    EXPECT_TRUE(server_->waitForClient());
  }

  void TearDown() override
  {
    client_.reset();
    server_.reset();
  }

  std::unique_ptr<FakePrimaryServer> server_;
  std::unique_ptr<primary_interface::PrimaryClient> client_;
  comm::INotifier notifier_;
};

TEST_F(PrimaryClientTest, start_communication_succeeds)
{
  EXPECT_NO_THROW(client_->start());
}

TEST_F(PrimaryClientTest, add_and_remove_consumer)
{
  URCL_SILENCE_DEPRECATED_BEGIN
  auto calibration_consumer = std::make_shared<urcl::CalibrationChecker>("test");
  URCL_SILENCE_DEPRECATED_END

  client_->addPrimaryConsumer(calibration_consumer);

  EXPECT_NO_THROW(client_->start());

  auto start_time = std::chrono::system_clock::now();
  const auto timeout = std::chrono::seconds(5);
  while (!calibration_consumer->isChecked() && std::chrono::system_clock::now() - start_time < timeout)
  {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }
  EXPECT_TRUE(calibration_consumer->isChecked());

  client_->removePrimaryConsumer(calibration_consumer);
}

TEST_F(PrimaryClientTest, test_power_cycle_commands)
{
  EXPECT_NO_THROW(client_->start());
  EXPECT_NO_THROW(client_->commandPowerOff());
  EXPECT_NO_THROW(client_->commandPowerOn());
  EXPECT_NO_THROW(client_->commandBrakeRelease());
  EXPECT_NO_THROW(client_->commandPowerOff());
  EXPECT_NO_THROW(client_->commandBrakeRelease());
  EXPECT_NO_THROW(client_->commandPowerOff());

  auto timeout = std::chrono::seconds(30);
  // provoke a timeout
  EXPECT_THROW(client_->commandBrakeRelease(true, std::chrono::milliseconds(1)), urcl::TimeoutException);
  EXPECT_NO_THROW(waitFor([this]() { return client_->getRobotMode() == RobotMode::RUNNING; }, timeout));
  EXPECT_THROW(client_->commandPowerOff(true, std::chrono::milliseconds(1)), urcl::TimeoutException);
  EXPECT_NO_THROW(waitFor([this]() { return client_->getRobotMode() == RobotMode::POWER_OFF; }, timeout));
  EXPECT_THROW(client_->commandPowerOn(true, std::chrono::milliseconds(1)), urcl::TimeoutException);
  EXPECT_NO_THROW(waitFor([this]() { return client_->getRobotMode() == RobotMode::IDLE; }, timeout));

  // Without a verification the calls should succeed, the robot ending up in the desired state
  // eventually.
  EXPECT_NO_THROW(client_->commandPowerOff(false));
  EXPECT_NO_THROW(waitFor([this]() { return client_->getRobotMode() == RobotMode::POWER_OFF; }, timeout));
  EXPECT_NO_THROW(client_->commandPowerOn(false));
  EXPECT_NO_THROW(waitFor([this]() { return client_->getRobotMode() == RobotMode::IDLE; }, timeout));
  EXPECT_NO_THROW(client_->commandBrakeRelease(false));
  EXPECT_NO_THROW(waitFor([this]() { return client_->getRobotMode() == RobotMode::RUNNING; }, timeout));
}

TEST_F(PrimaryClientTest, test_power_on_when_already_idle)
{
  EXPECT_NO_THROW(client_->start());
  EXPECT_NO_THROW(client_->commandPowerOff());
  EXPECT_NO_THROW(client_->commandPowerOn());

  ASSERT_EQ(client_->getRobotMode(), RobotMode::IDLE);

  EXPECT_NO_THROW(client_->commandPowerOn());
  EXPECT_EQ(client_->getRobotMode(), RobotMode::IDLE);
}

TEST_F(PrimaryClientTest, test_power_on_when_already_running)
{
  EXPECT_NO_THROW(client_->start());
  EXPECT_NO_THROW(client_->commandPowerOff());
  EXPECT_NO_THROW(client_->commandBrakeRelease());

  ASSERT_EQ(client_->getRobotMode(), RobotMode::RUNNING);

  EXPECT_NO_THROW(client_->commandPowerOn());
  EXPECT_EQ(client_->getRobotMode(), RobotMode::RUNNING);
}

TEST_F(PrimaryClientTest, test_unlock_protective_stop)
{
  EXPECT_NO_THROW(client_->start());
  EXPECT_NO_THROW(client_->commandBrakeRelease(true, std::chrono::milliseconds(20000)));
  client_->sendScript("protective_stop()");
  EXPECT_NO_THROW(waitFor([this]() { return client_->isRobotProtectiveStopped(); }, std::chrono::milliseconds(1000)));
  // This will not happen immediately
  EXPECT_THROW(client_->commandUnlockProtectiveStop(true, std::chrono::milliseconds(1)), TimeoutException);

  // It is not allowed to unlock the protective stop immediately
  std::this_thread::sleep_for(std::chrono::seconds(5));
  EXPECT_NO_THROW(client_->commandUnlockProtectiveStop());
}

TEST_F(PrimaryClientTest, test_uninitialized_primary_client)
{
  // The client is not started yet, so the robot mode should be UNKNOWN
  ASSERT_EQ(client_->getRobotMode(), RobotMode::UNKNOWN);
  ASSERT_THROW(client_->isRobotProtectiveStopped(), UrException);
  // The client is not started yet, so the robot type should be UNDEFINED
  ASSERT_EQ(client_->getRobotType(), RobotType::UNDEFINED);
  // Without a robot type (and version), the robot series cannot be determined.
  ASSERT_EQ(client_->getRobotSeries(), RobotSeries::UNDEFINED);
}

TEST_F(PrimaryClientTest, test_stop_command)
{
  // Without started communication the latest robot mode data is a nullptr
  EXPECT_THROW(client_->commandStop(), UrException);

  EXPECT_NO_THROW(client_->start());
  EXPECT_NO_THROW(client_->commandPowerOff());
  EXPECT_NO_THROW(client_->commandBrakeRelease());

  const std::string script_code = "def test_fun():\n"
                                  "  while True:\n"
                                  "     textmsg(\"still running\")\n"
                                  "     sleep(1.0)\n"
                                  "     sync()\n"
                                  "  end\n"
                                  "end";

  EXPECT_TRUE(client_->sendScript(script_code));
  waitFor([this]() { return client_->getRobotModeData()->is_program_running_; }, std::chrono::seconds(5));

  EXPECT_NO_THROW(client_->commandStop());
  EXPECT_FALSE(client_->getRobotModeData()->is_program_running_);

  // Without a program running it should not throw an exception
  EXPECT_NO_THROW(client_->commandStop());

  EXPECT_TRUE(client_->sendScript(script_code));
  waitFor([this]() { return client_->getRobotModeData()->is_program_running_; }, std::chrono::seconds(5));
  EXPECT_THROW(client_->commandStop(true, std::chrono::milliseconds(1)), TimeoutException);
  EXPECT_NO_THROW(waitFor(
      [this]() {
        return !client_->getRobotModeData()->is_program_running_ && !client_->getRobotModeData()->is_program_paused_;
      },
      std::chrono::seconds(5)));

  // without validation
  EXPECT_TRUE(client_->sendScript(script_code));
  waitFor([this]() { return client_->getRobotModeData()->is_program_running_; }, std::chrono::seconds(5));
  EXPECT_NO_THROW(client_->commandStop(false));
  EXPECT_NO_THROW(waitFor(
      [this]() {
        return !client_->getRobotModeData()->is_program_running_ && !client_->getRobotModeData()->is_program_paused_;
      },
      std::chrono::seconds(5)));
}

TEST_F(PrimaryClientTest, test_configuration_data)
{
  EXPECT_NO_THROW(client_->start());

  // Once we connect to the primary client we should receive configuration data
  auto start_time = std::chrono::system_clock::now();
  const auto timeout = std::chrono::seconds(1);
  auto config_data = client_->getConfigurationData();
  while (config_data == nullptr && std::chrono::system_clock::now() - start_time < timeout)
  {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    config_data = client_->getConfigurationData();
  }

  // We should have received a configuration data package
  EXPECT_NE(config_data, nullptr);

  // Robot type should no longer be undefined once we have received configuration data.
  EXPECT_NE(client_->getRobotType(), RobotType::UNDEFINED);
}

TEST_F(PrimaryClientTest, test_robot_type)
{
  if (std::getenv("ROBOT_MODEL") == nullptr)
  {
    GTEST_SKIP() << "ROBOT_MODEL environment variable not set. Skipping test.";
  }
  // When this test runs as the only test against a freshly started URSim (as it does in the
  // robot_model_check CI matrix), the robot may still be initialising and drop the first
  // primary connection. Use a short socket reconnection interval so that the producer retries
  // quickly, and a generous outer timeout so we can ride out URSim's full warm-up.
  constexpr auto reconnection_time = std::chrono::milliseconds(500);
  EXPECT_NO_THROW(client_->start(/*max_num_tries=*/0, reconnection_time));

  // Wait until we have received configuration data so that the robot type is known.
  const auto start_time = std::chrono::system_clock::now();
  const auto timeout = std::chrono::seconds(60);
  while (client_->getConfigurationData() == nullptr && std::chrono::system_clock::now() - start_time < timeout)
  {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }
  ASSERT_NE(client_->getConfigurationData(), nullptr);
  const std::string robot_model_env = std::getenv("ROBOT_MODEL");
  const RobotType expected_robot_type = robotTypeFromString(robot_model_env);
  EXPECT_EQ(client_->getRobotType(), expected_robot_type);
}

TEST_F(PrimaryClientTest, test_kinematics_info)
{
  EXPECT_NO_THROW(client_->start());

  // Once we connect to the primary client we should receive kinematics info
  auto start_time = std::chrono::system_clock::now();
  const auto timeout = std::chrono::seconds(1);
  auto kinematics_info = client_->getKinematicsInfo();
  while (kinematics_info == nullptr && std::chrono::system_clock::now() - start_time < timeout)
  {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    kinematics_info = client_->getKinematicsInfo();
  }

  // We should have received a kinematics info package
  EXPECT_NE(kinematics_info, nullptr);
}

TEST_F(PrimaryClientTest, test_get_robot_series)
{
  EXPECT_NO_THROW(client_->start());

  // Wait until we have received configuration data so that the robot type is known.
  auto start_time = std::chrono::system_clock::now();
  const auto timeout = std::chrono::seconds(2);
  while (client_->getConfigurationData() == nullptr && std::chrono::system_clock::now() - start_time < timeout)
  {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }
  ASSERT_NE(client_->getConfigurationData(), nullptr);

  const RobotType robot_type = client_->getRobotType();
  ASSERT_NE(robot_type, RobotType::UNDEFINED);

  auto version_info = client_->getRobotVersion();
  ASSERT_NE(version_info, nullptr);

  // The series returned by the client must match what robotSeriesFromTypeAndVersion computes
  // from the type and version the client itself reports.
  const RobotSeries expected_series = robotSeriesFromTypeAndVersion(robot_type, *version_info);
  EXPECT_EQ(client_->getRobotSeries(), expected_series);

  // A robot we successfully connected to should map to a known series.
  EXPECT_NE(client_->getRobotSeries(), RobotSeries::UNDEFINED);
}

TEST_F(PrimaryClientTest, test_program_execution)
{
  auto consumer = std::make_shared<RobotMessageConsumer>();

  client_->addPrimaryConsumer(consumer);

  EXPECT_NO_THROW(client_->start());
  EXPECT_NO_THROW(client_->commandPowerOff());
  EXPECT_NO_THROW(client_->commandBrakeRelease());

  const std::string script_code = "def test_fun():\n"
                                  "  textmsg(\"still running\")\n"
                                  "  sleep(0.1)\n"
                                  "  sync()\n"
                                  "end";

  EXPECT_TRUE(client_->sendScript(script_code));

  {  // we get a key message that the program started
    bool answer_received = false;
    while (!answer_received)
    {
      auto message = consumer->getOrWaitForMessage();
      auto key_message = std::dynamic_pointer_cast<primary_interface::KeyMessage>(message);
      if (key_message)
      {
        answer_received = true;
        EXPECT_EQ(key_message->title_, "PROGRAM_XXX_STARTED");
        EXPECT_EQ(key_message->text_, "test_fun");
      }
    }
  }

  {  // we get the textmessage of the program
    bool answer_received = false;
    while (!answer_received)
    {
      auto message = consumer->getOrWaitForMessage();
      auto text_message = std::dynamic_pointer_cast<primary_interface::TextMessage>(message);
      if (text_message)
      {
        answer_received = true;
        EXPECT_EQ(text_message->text_, "still running");
      }
    }
  }

  {  // we get a key message that the program stopped
    bool answer_received = false;
    while (!answer_received)
    {
      auto message = consumer->getOrWaitForMessage();
      auto key_message = std::dynamic_pointer_cast<primary_interface::KeyMessage>(message);
      if (key_message)
      {
        answer_received = true;
        EXPECT_EQ(key_message->title_, "PROGRAM_XXX_STOPPED");
        EXPECT_EQ(key_message->text_, "test_fun");
      }
    }
  }
}

TEST_F(PrimaryClientTest, test_program_execution_reports_exception)
{
  auto consumer = std::make_shared<RobotMessageConsumer>();

  client_->addPrimaryConsumer(consumer);

  EXPECT_NO_THROW(client_->start());
  EXPECT_NO_THROW(client_->commandPowerOff());
  EXPECT_NO_THROW(client_->commandBrakeRelease());

  const std::string script_code = "def illegal_fun():\n"
                                  "  calldoesntexist()\n"
                                  "end";

  EXPECT_TRUE(client_->sendScript(script_code));

  {  // we get a RuntimeException message saying that out function doesn't exist
    bool answer_received = false;
    while (!answer_received)
    {
      auto message = consumer->getOrWaitForMessage();
      auto typed_msg = std::dynamic_pointer_cast<primary_interface::RuntimeExceptionMessage>(message);
      if (typed_msg)
      {
        answer_received = true;
        EXPECT_EQ(typed_msg->line_number_, 2);
        EXPECT_EQ(typed_msg->column_number_, 3);
        EXPECT_EQ(typed_msg->text_, "compile_error_name_not_found:calldoesntexist:");
      }
    }
  }
}

TEST_F(PrimaryClientTest, test_read_safety_mode)
{
  EXPECT_NO_THROW(client_->start());
  EXPECT_EQ(client_->getSafetyMode(), urcl::SafetyMode::UNDEFINED_SAFETY_MODE);
  EXPECT_NO_THROW(client_->commandBrakeRelease(true, std::chrono::milliseconds(20000)));
  client_->sendScript("protective_stop()");
  EXPECT_NO_THROW(waitFor([this]() { return client_->getSafetyMode() == urcl::SafetyMode::PROTECTIVE_STOP; },
                          std::chrono::milliseconds(1000)));
  // This will not happen immediately
  EXPECT_THROW(client_->commandUnlockProtectiveStop(true, std::chrono::milliseconds(1)), TimeoutException);

  // It is not allowed to unlock the protective stop immediately
  std::this_thread::sleep_for(std::chrono::seconds(5));
  EXPECT_NO_THROW(client_->commandUnlockProtectiveStop());
  EXPECT_EQ(client_->getSafetyMode(), urcl::SafetyMode::NORMAL);
}

TEST_F(PrimaryClientTest, test_send_script_blocking_happy_path)
{
  EXPECT_NO_THROW(client_->start());
  EXPECT_NO_THROW(client_->commandPowerOff());
  EXPECT_NO_THROW(client_->commandBrakeRelease());

  const std::string fully_defined_script = "def test_fun():\n"
                                           "  textmsg(\"still running\")\n"
                                           "  sleep(0.1)\n"
                                           "  sync()\n"
                                           "end";
  EXPECT_NO_THROW(client_->sendScriptBlocking(fully_defined_script));

  const std::string part_defined_script = "textmsg(\"still running\")\n"
                                          "sleep(0.1)\n"
                                          "sync()\n";
  EXPECT_NO_THROW(client_->sendScriptBlocking(part_defined_script));
  EXPECT_NO_THROW(client_->sendScriptBlocking(part_defined_script, "test_def"));
  std::string sec_script = "sec test_sec():\n  textmsg(\"Still running\")\nend";
  EXPECT_NO_THROW(client_->sendScriptBlocking(sec_script, "test_sec"));
}

TEST_F(PrimaryClientTest, test_send_script_blocking_fails_on_nonrunning_robot)
{
  EXPECT_NO_THROW(client_->start());
  EXPECT_NO_THROW(client_->commandPowerOff());
  EXPECT_THROW(client_->sendScriptBlocking("textmsg(\"Still running\")"), RobotModeException);
  EXPECT_NO_THROW(client_->commandPowerOn());
  EXPECT_THROW(client_->sendScriptBlocking("textmsg(\"Still running\")"), RobotModeException);
  EXPECT_NO_THROW(client_->commandBrakeRelease());
  EXPECT_NO_THROW(client_->sendScriptBlocking("textmsg(\"Still running\")"));
}

TEST_F(PrimaryClientTest, test_send_script_blocking_fails_on_bad_safety_mode)
{
  EXPECT_NO_THROW(client_->start());
  EXPECT_NO_THROW(client_->commandPowerOff());
  EXPECT_NO_THROW(client_->commandBrakeRelease());
  ASSERT_TRUE(client_->safetyModeAllowsExecution());

  EXPECT_THROW(client_->sendScriptBlocking("protective_stop()"), RobotErrorCodeException);
  EXPECT_THROW(client_->sendScriptBlocking("textmsg(\"Still running\")"), SafetyModeException);
  EXPECT_NO_THROW(client_->commandUnlockProtectiveStop());
  EXPECT_NO_THROW(client_->sendScriptBlocking("textmsg(\"Still running\")"));
}

TEST_F(PrimaryClientTest, test_send_script_blocking_throw_on_malformed_scripts)
{
  EXPECT_NO_THROW(client_->start());
  const std::string script_no_end = "def test_fun():\n"
                                    "  textmsg(\"testing\")";
  EXPECT_THROW(client_->sendScriptBlocking(script_no_end), urcl::ScriptCodeSyntaxException);
  const std::string script_bad_name = "def 7_eight_9():\n"
                                      "  textmsg(\"testing\")\n"
                                      "end";
  EXPECT_THROW(client_->sendScriptBlocking(script_bad_name), urcl::ScriptCodeSyntaxException);
  EXPECT_THROW(client_->sendScriptBlocking("textmsg(\"testing\")", "0_errors"), urcl::ScriptCodeSyntaxException);
  const std::string comments_only = "#only\n#comments\n\n\n#and\n#whitespace";
  EXPECT_THROW(client_->sendScriptBlocking(comments_only), urcl::ScriptCodeSyntaxException);
  const std::string script_no_paren = "def test_fun:\n"
                                      "  textmsg(\"testing\")"
                                      "end";
  EXPECT_THROW(client_->sendScriptBlocking(script_no_paren), urcl::ScriptCodeSyntaxException);
}

TEST_F(PrimaryClientTest, test_send_script_blocking_fail_on_runtime_exception)
{
  EXPECT_NO_THROW(client_->start());
  EXPECT_NO_THROW(client_->commandPowerOff());
  EXPECT_NO_THROW(client_->commandBrakeRelease());
  // Non-invertible goal, should throw runtime exception
  EXPECT_THROW(client_->sendScriptBlocking("movej(p[10,0,0,0,0,0])"), RobotRuntimeException);
}

TEST_F(PrimaryClientTest, test_send_script_blocking_fail_on_robot_errors)
{
  EXPECT_NO_THROW(client_->start());
  EXPECT_NO_THROW(client_->commandPowerOff());
  EXPECT_NO_THROW(client_->commandBrakeRelease());
  // Impossible movement, will trigger a warning and protective stop
  EXPECT_THROW(client_->sendScriptBlocking("movel(p[10,0,0,0,0,0])"), RobotErrorCodeException);
  // reset the robot
  ASSERT_NO_THROW(client_->commandUnlockProtectiveStop());
  EXPECT_NO_THROW(client_->sendScriptBlocking("movej([0.5,-0.5,0.5,0,0,0])"));
}

TEST_F(PrimaryClientTest, test_send_script_blocking_fail_on_bad_script)
{
  EXPECT_NO_THROW(client_->start());
  EXPECT_NO_THROW(client_->commandPowerOff());
  EXPECT_NO_THROW(client_->commandBrakeRelease());

  EXPECT_THROW(client_->sendScriptBlocking("non_existing_func()"), RobotRuntimeException);

  const std::string script_code = "def illegal_fun():\n"
                                  "  calldoesntexist()\n"
                                  "end";

  EXPECT_THROW(client_->sendScriptBlocking(script_code), RobotRuntimeException);
}

TEST_F(PrimaryClientTest, test_send_script_blocking_ignore_warnings)
{
  EXPECT_NO_THROW(client_->start());
  EXPECT_NO_THROW(client_->commandPowerOff());
  EXPECT_NO_THROW(client_->commandBrakeRelease());
  // Trigger protective stop (warning level error code)
  EXPECT_NO_THROW(client_->sendScriptBlocking("protective_stop()", "", std::chrono::milliseconds(1000), false));
  // reset the robot
  ASSERT_NO_THROW(client_->commandUnlockProtectiveStop());
}

TEST_F(PrimaryClientTest, test_send_script_blocking_replace_long_names)
{
  EXPECT_NO_THROW(client_->start());
  EXPECT_NO_THROW(client_->commandPowerOff());
  EXPECT_NO_THROW(client_->commandBrakeRelease());
  const std::string name = "this_is_a_very_long_script_name_that_should_be_truncated";
  EXPECT_NO_THROW(client_->sendScriptBlocking("textmsg(\"Still running\")", name));
  const std::string long_name_script = "def " + name +
                                       "():\n"
                                       "  textmsg(\"still running\")\n"
                                       "  sleep(0.1)\n"
                                       "  sync()\n"
                                       "end";
  EXPECT_NO_THROW(client_->sendScriptBlocking(long_name_script));
}

TEST_F(PrimaryClientFakeTest, test_send_script_blocking_fail_on_missing_robot_mode)
{
  // We did NOT send a robot mode, yet.

  EXPECT_THROW(
      client_->sendScriptBlocking("textmsg(\"Still running\")", "test_fun", std::chrono::milliseconds(1000), false),
      TimeoutException);

  server_->setScriptCallback([this]([[maybe_unused]] const std::string& payload) {
    server_->sendKeyMessage("PROGRAM_XXX_STARTED", "test_fun");
    std::this_thread::sleep_for(std::chrono::milliseconds(20));
    server_->sendKeyMessage("PROGRAM_XXX_STOPPED", "test_fun");
  });
  std::thread delayed_robot_mode_thread([this]() {
    // We will send the robot mode data and program started message after a short delay, which should allow the
    // sendScriptBlocking call to succeed even though it initially times out waiting for robot mode data.
    // std::this_thread::sleep_for(std::chrono::milliseconds(200));
    server_->sendRobotModeData(RobotMode::RUNNING, true, true, true, false, false, false, false);
  });
  EXPECT_NO_THROW(
      client_->sendScriptBlocking("textmsg(\"Still running\")", "test_fun", std::chrono::milliseconds(1000), false));

  if (delayed_robot_mode_thread.joinable())
  {
    delayed_robot_mode_thread.join();
  }
}

TEST_F(PrimaryClientFakeTest, test_send_script_blocking_fail_on_fault)
{
  server_->sendRobotModeData(RobotMode::RUNNING, true, true, true, false, false, false, false);

  const std::string script_code = "textmsg(\"Still running\")";

  // Make the fake server send an error code message with severity fault when it receives a script
  server_->setScriptCallback([this, script_code](const std::string& payload) {
    if (payload.find(script_code) != std::string::npos)
    {
      server_->sendKeyMessage("PROGRAM_XXX_STARTED", "test_fun");
      ASSERT_EQ(payload, "def test_fun():\n  " + script_code + "\nend\n\n");
      ASSERT_TRUE(server_->sendErrorCodeMessage(999, 0, ReportLevel::FAULT, "Simulated fault"));
    }
    else
    {
      // We expect that the primary client calls "stop program" when it receives a fault
      ASSERT_EQ(payload, "stop program\n");
    }
  });

  URCL_LOG_INFO("Sending script that will trigger a fault on the fake server");
  EXPECT_THROW(client_->sendScriptBlocking(script_code, "test_fun", std::chrono::milliseconds(1000), false),
               RobotErrorCodeException);
}

TEST_F(PrimaryClientFakeTest, test_send_script_to_read_only_server)
{
  server_->sendRobotModeData(RobotMode::RUNNING, true, true, true, false, false, false, false);

  const std::string script_code = "textmsg(\"Still running\")";

  // Make the fake server send an error code message with code 210 (read-only primary interface) when it receives a
  // script
  server_->setScriptCallback([this, script_code]([[maybe_unused]] const std::string& payload) {
    if (payload.find(script_code) != std::string::npos)
    {
      ASSERT_TRUE(
          server_->sendErrorCodeMessage(210, 0, ReportLevel::VIOLATION, "Simulated read-only primary interface error"));
    }
  });

  // Fails even with retry
  EXPECT_THROW(client_->sendScriptBlocking(script_code, "test_fun", std::chrono::milliseconds(1000), false, true),
               ReadOnlyInterfaceException);

  bool retry = false;
  // Send C210 on first try, then succeed
  server_->setScriptCallback([this, script_code, &retry]([[maybe_unused]] const std::string& payload) {
    if (!retry && payload.find(script_code) != std::string::npos)
    {
      ASSERT_TRUE(
          server_->sendErrorCodeMessage(210, 0, ReportLevel::VIOLATION, "Simulated read-only primary interface error"));
      retry = true;
    }
    else if (payload.find(script_code) != std::string::npos)
    {
      server_->sendKeyMessage("PROGRAM_XXX_STARTED", "test_fun");
      ASSERT_EQ(payload, "def test_fun():\n  " + script_code + "\nend\n\n");
      server_->sendKeyMessage("PROGRAM_XXX_STOPPED", "test_fun");
    }
  });

  // Fails with no retry
  EXPECT_THROW(client_->sendScriptBlocking(script_code, "test_fun", std::chrono::milliseconds(1000), false, false),
               ReadOnlyInterfaceException);

  retry = false;
  // Succeeds on retry
  EXPECT_NO_THROW(client_->sendScriptBlocking(script_code, "test_fun", std::chrono::milliseconds(1000), false, true));
}

TEST_F(PrimaryClientFakeTest, test_send_script_blocking_timeout_on_no_response)
{
  server_->sendRobotModeData(RobotMode::RUNNING, true, true, true, false, false, false, false);

  const std::string script_code = "textmsg(\"Still running\")";

  // We do not set a script callback on the fake server, so it will not respond to the script being sent. This should
  // cause sendScriptBlocking to time out and throw a TimeoutException.
  EXPECT_THROW(client_->sendScriptBlocking(script_code, "test_fun", std::chrono::milliseconds(100), false),
               TimeoutException);
}

TEST_F(PrimaryClientFakeTest, test_error_code_report_levels_get_handled_correctly)
{
  ASSERT_TRUE(server_->sendErrorCodeMessage(999, 0, ReportLevel::DEBUG, "Simulated Debug"));
  ASSERT_TRUE(server_->sendErrorCodeMessage(999, 0, ReportLevel::INFO, "Simulated Info"));
  ASSERT_TRUE(server_->sendErrorCodeMessage(999, 0, ReportLevel::WARNING, "Simulated Warning"));
  ASSERT_TRUE(server_->sendErrorCodeMessage(999, 0, ReportLevel::VIOLATION, "Simulated Violation"));
  ASSERT_TRUE(server_->sendErrorCodeMessage(999, 0, ReportLevel::FAULT, "Simulated Fault"));
  ASSERT_TRUE(server_->sendErrorCodeMessage(999, 0, ReportLevel::CRITICAL_FAULT, "Simulated Critical Fault"));
  ASSERT_TRUE(server_->sendErrorCodeMessage(999, 0, ReportLevel::DEVL_DEBUG, "Simulated DEVL Debug"));
  ASSERT_TRUE(server_->sendErrorCodeMessage(999, 0, ReportLevel::DEVL_INFO, "Simulated DEVL Info"));
  ASSERT_TRUE(server_->sendErrorCodeMessage(999, 0, ReportLevel::DEVL_WARNING, "Simulated DEVL Warning"));
  ASSERT_TRUE(server_->sendErrorCodeMessage(999, 0, ReportLevel::DEVL_VIOLATION, "Simulated DEVL Violation"));
  ASSERT_TRUE(server_->sendErrorCodeMessage(999, 0, ReportLevel::DEVL_FAULT, "Simulated DEVL Fault"));
  ASSERT_TRUE(server_->sendErrorCodeMessage(999, 0, ReportLevel::DEVL_CRITICAL_FAULT, "Simulated DEVL Critical Fault"));

  auto errors = client_->getErrorCodes();

  // Wait until we asynchronously receive all the error codes we sent.
  waitFor(
      [&errors, this]() {
        auto new_errors = client_->getErrorCodes();
        errors.insert(errors.end(), new_errors.begin(), new_errors.end());
        return errors.size() >= 12;
      },
      std::chrono::seconds(1));

  EXPECT_EQ(errors[0].report_level, ReportLevel::DEBUG);
  EXPECT_EQ(errors[1].report_level, ReportLevel::INFO);
  EXPECT_EQ(errors[2].report_level, ReportLevel::WARNING);
  EXPECT_EQ(errors[3].report_level, ReportLevel::VIOLATION);
  EXPECT_EQ(errors[4].report_level, ReportLevel::FAULT);
  EXPECT_EQ(errors[5].report_level, ReportLevel::CRITICAL_FAULT);
  EXPECT_EQ(errors[6].report_level, ReportLevel::DEVL_DEBUG);
  EXPECT_EQ(errors[7].report_level, ReportLevel::DEVL_INFO);
  EXPECT_EQ(errors[8].report_level, ReportLevel::DEVL_WARNING);
  EXPECT_EQ(errors[9].report_level, ReportLevel::DEVL_VIOLATION);
  EXPECT_EQ(errors[10].report_level, ReportLevel::DEVL_FAULT);
  EXPECT_EQ(errors[11].report_level, ReportLevel::DEVL_CRITICAL_FAULT);
}

int main(int argc, char* argv[])
{
  ::testing::InitGoogleTest(&argc, argv);
  for (int i = 0; i < argc; i++)
  {
    if (std::string(argv[i]) == "--robot_ip" && i + 1 < argc)
    {
      g_ROBOT_IP = argv[i + 1];
      ++i;
    }
  }

  return RUN_ALL_TESTS();
}