// test_custom_interfaces.cpp - gtest 验证自定义消息构造
#include <cmath>
#include <memory>

#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"

#include "cpp_custom_interface/msg/sensor_reading.hpp"
#include "cpp_custom_interface/srv/get_calibration.hpp"
#include "cpp_custom_interface/action/move_arm.hpp"

class CustomInterfaceTest : public ::testing::Test
{
protected:
  static void SetUpTestSuite() { rclcpp::init(0, nullptr); }
  static void TearDownTestSuite() { rclcpp::shutdown(); }
};

TEST_F(CustomInterfaceTest, SensorReadingFields)
{
  auto msg = cpp_custom_interface::msg::SensorReading();
  msg.sensor_id = "imu_0";
  msg.unit = "rad/s";
  msg.value = 1.234;

  EXPECT_EQ(msg.sensor_id, "imu_0");
  EXPECT_EQ(msg.unit, "rad/s");
  EXPECT_DOUBLE_EQ(msg.value, 1.234);
}

TEST_F(CustomInterfaceTest, GetCalibrationRequestResponse)
{
  auto req = std::make_shared<cpp_custom_interface::srv::GetCalibration::Request>();
  req->sensor_id = "lidar_front";

  auto resp = std::make_shared<cpp_custom_interface::srv::GetCalibration::Response>();
  resp->intrinsic_matrix = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
  resp->bias = {0.1, 0.2, 0.3};
  resp->calibration_date = "2026-08-04";
  resp->valid = true;

  EXPECT_EQ(req->sensor_id, "lidar_front");
  EXPECT_TRUE(resp->valid);
  EXPECT_EQ(resp->intrinsic_matrix.size(), 9u);
  EXPECT_EQ(resp->bias.size(), 3u);
}

TEST_F(CustomInterfaceTest, MoveArmGoalFeedbackResult)
{
  auto goal = cpp_custom_interface::action::MoveArm::Goal();
  goal.max_velocity_scaling = 0.5f;

  auto fb = cpp_custom_interface::action::MoveArm::Feedback();
  fb.progress = 0.5f;
  fb.current_state = "executing";

  auto result = cpp_custom_interface::action::MoveArm::Result();
  result.success = true;
  result.total_time_sec = 1.5;

  EXPECT_FLOAT_EQ(goal.max_velocity_scaling, 0.5f);
  EXPECT_FLOAT_EQ(fb.progress, 0.5f);
  EXPECT_TRUE(result.success);
}