#include <gtest/gtest.h>

#include <fstream>
#include <set>
#include <string>

#include "obstacle_nav2/nav2_profile_loader.hpp"

namespace
{

std::string writeTempProfile(const std::string & yaml)
{
  const auto path = "/tmp/nav2_profile_loader_test.yaml";
  std::ofstream out(path);
  out << yaml;
  out.close();
  return path;
}

}  // namespace

TEST(Nav2ProfileLoaderTest, LoadsScalarAndArrayParameters)
{
  const auto path = writeTempProfile(R"(
nodes:
  /controller_server:
    FollowPath.vx_max: 0.35
    FollowPath.vx_min: -0.10
    FollowPath.wz_max: 1.2
    FollowPath.AckermannConstraints.min_turning_r: 0.75
  /velocity_smoother:
    max_velocity: [0.35, 0.0, 1.2]
    min_velocity: [-0.10, 0.0, -1.2]
)");

  const auto profile = obstacle_nav2::loadNav2Profile(path);

  ASSERT_EQ(profile.nodes.size(), 2u);
  EXPECT_EQ(profile.nodes[0].node_name, "/controller_server");
  ASSERT_EQ(profile.nodes[0].parameters.size(), 4u);
  EXPECT_EQ(profile.nodes[0].parameters[0].get_name(), "FollowPath.vx_max");
  EXPECT_DOUBLE_EQ(profile.nodes[0].parameters[0].as_double(), 0.35);
  EXPECT_EQ(
    profile.nodes[0].parameters[3].get_name(),
    "FollowPath.AckermannConstraints.min_turning_r");
  EXPECT_DOUBLE_EQ(profile.nodes[0].parameters[3].as_double(), 0.75);

  EXPECT_EQ(profile.nodes[1].node_name, "/velocity_smoother");
  ASSERT_EQ(profile.nodes[1].parameters.size(), 2u);
  const auto max_velocity = profile.nodes[1].parameters[0].as_double_array();
  ASSERT_EQ(max_velocity.size(), 3u);
  EXPECT_DOUBLE_EQ(max_velocity[0], 0.35);
  EXPECT_DOUBLE_EQ(max_velocity[2], 1.2);
}

TEST(Nav2ProfileLoaderTest, LoadsNativeNav2RosParametersFile)
{
  const auto path = writeTempProfile(R"(
bt_navigator:
  ros__parameters:
    default_server_timeout: 20
controller_server:
  ros__parameters:
    controller_frequency: 20.0
    FollowPath:
      vx_max: 0.35
      wz_max: 1.2
      motion_model: "Ackermann"
      critics: ["ConstraintCritic", "CostCritic"]
      AckermannConstraints:
        min_turning_r: 0.75
velocity_smoother:
  ros__parameters:
    max_velocity: [0.35, 0.0, 1.2]
    min_velocity: [-0.10, 0.0, -1.2]
)");

  const auto profile = obstacle_nav2::loadNav2Profile(path);

  ASSERT_EQ(profile.nodes.size(), 2u);
  EXPECT_EQ(profile.nodes[0].node_name, "/controller_server");
  ASSERT_EQ(profile.nodes[0].parameters.size(), 6u);
  EXPECT_EQ(profile.nodes[0].parameters[1].get_name(), "FollowPath.vx_max");
  EXPECT_DOUBLE_EQ(profile.nodes[0].parameters[1].as_double(), 0.35);
  EXPECT_EQ(
    profile.nodes[0].parameters[5].get_name(),
    "FollowPath.AckermannConstraints.min_turning_r");
  EXPECT_DOUBLE_EQ(profile.nodes[0].parameters[5].as_double(), 0.75);

  EXPECT_EQ(profile.nodes[1].node_name, "/velocity_smoother");
  ASSERT_EQ(profile.nodes[1].parameters.size(), 2u);
  const auto max_velocity = profile.nodes[1].parameters[0].as_double_array();
  ASSERT_EQ(max_velocity.size(), 3u);
  EXPECT_DOUBLE_EQ(max_velocity[2], 1.2);
}

TEST(Nav2ProfileLoaderTest, RejectsProfileWithoutNodesMap)
{
  const auto path = writeTempProfile(R"(
controller_server:
  FollowPath.vx_max: 0.35
)");

  EXPECT_THROW(
    obstacle_nav2::loadNav2Profile(path),
    std::runtime_error);
}

TEST(Nav2ProfileLoaderTest, FiltersParametersByDeclaredNames)
{
  const std::vector<rclcpp::Parameter> parameters{
    rclcpp::Parameter("FollowPath.vx_max", 0.35),
    rclcpp::Parameter("FollowPath.CostCritic.trajectory_point_step", 2),
    rclcpp::Parameter("max_velocity", std::vector<double>{0.35, 0.0, 1.2}),
  };
  const std::set<std::string> declared_names{
    "FollowPath.vx_max",
    "max_velocity",
  };

  const auto filtered = obstacle_nav2::filterDeclaredParameters(parameters, declared_names);

  ASSERT_EQ(filtered.size(), 2u);
  EXPECT_EQ(filtered[0].get_name(), "FollowPath.vx_max");
  EXPECT_EQ(filtered[1].get_name(), "max_velocity");
}
