/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * recovery_core — Boost.DLL plugin contract test.
 *
 * Author: DuongTD
 *********************************************************************/

#include <recovery_core/recovery_behavior.h>

#include <cmath>
#include <cstdlib>
#include <iostream>
#include <string>
#include <vector>
#include <robot/robot.h>
#include <robot_xmlrpcpp/XmlRpcValue.h>

#include <boost/dll/import.hpp>
#include <boost/dll/shared_library.hpp>
#include <boost/filesystem/path.hpp>

namespace
{
using Factory = recovery_core::RecoveryBehavior::RecoveryBehaviorPtr();

struct PluginConfig
{
    std::string name;
    std::string type;
};
std::vector<boost::dll::shared_library> libraries_;
std::vector<recovery_core::RecoveryBehavior::RecoveryBehaviorPtr> creators_;
std::vector<std::string> name_plugins_;

std::vector<robot_geometry_msgs::PoseStamped> global_path_;

void expect(bool condition, const std::string& message)
{
  if (!condition)
  {
    std::cerr << "[FAIL] " << message << std::endl;
    std::exit(1);
  }
}

std::vector<PluginConfig> getListRecoveryPlugins()
{

std::vector<PluginConfig> my_list;

robot::NodeHandle priv_nh;

if (priv_nh.hasParam("recovery_behaviors"))
{
    YAML::Node my_plugins = priv_nh.getParamValue("recovery_behaviors");

    if (my_plugins.IsDefined() && my_plugins.IsSequence())
    {
       std::set<std::string> name_plugins;
        for (std::size_t i = 0; i < my_plugins.size(); ++i)
        {
            YAML::Node plugin_i = my_plugins[i];

            // 1. Phải là map
            if (!plugin_i.IsMap())
            {
                std::cerr<< "Recovery plugin at index " << i << " must be a map." << std::endl;
                continue;
            }

            // 2. Phải có name và type
            if (!plugin_i["name"].IsDefined() || !plugin_i["type"].IsDefined())
            {
                std::cerr << "Recovery plugin at index " << i << " must have 'name' and 'type'." << std::endl;
                continue;
            }

            PluginConfig p;

            try
            {
                p.name = plugin_i["name"].as<std::string>();
                p.type = plugin_i["type"].as<std::string>();
            }
            catch (const YAML::Exception& e)
            {
                std::cerr << "Invalid recovery plugin at index " << i << ": " << e.what() << std::endl;
                continue;
            }

            // 3. Kiểm tra duplicate name
            const auto result = name_plugins.insert(p.name);

            if (!result.second)
            {
                std::cerr << "A recovery plugin with name '" << p.name << "' already exists." << std::endl;
                continue;
            }

            // 4. Chỉ thêm sau khi validate hoàn toàn
            my_list.push_back(p);
            name_plugins_.push_back(p.name);

            robot::log_warning("Load plugin: name: %s, type: %s", p.name.c_str(), p.type.c_str());
        }
    }
    else
    {
        std::cerr << "'recovery_behaviors' must be a sequence." << std::endl;
    }
}

return my_list;
}

void testLoadPlugins()
{
       std::vector<PluginConfig> my_list_plugin = getListRecoveryPlugins();
       if(my_list_plugin.empty())
       {
              robot::log_error("No recovery plugins found in configuration.");
              return;
       }
       for(const auto& plugin : my_list_plugin)
       {
              robot::PluginLoaderHelper loader;
              std::string path_file_so = loader.findLibraryPath(plugin.type);
              if(path_file_so == "")
              {
                     robot::log_error("Cannot find library for recovery behavior type '%s'", plugin.type.c_str());
                     return;
              }
              robot::log_info("Loading recovery behavior type '%s' from '%s'", plugin.type.c_str(), path_file_so.c_str());
              try
              {
                     // 1. Load library vào local handle.
                     boost::dll::shared_library library(path_file_so);

                     // 2. Lấy factory alias.
                     auto& factory = library.get_alias<Factory>(plugin.type);

                     // 3. Factory tạo behavior object.
                     recovery_core::RecoveryBehavior::RecoveryBehaviorPtr behavior = factory();

                     if (!behavior)
                     {
                            robot::log_error("Factory returned nullptr for '%s'", plugin.type.c_str());
                            return;
                     }

                     // 4. Chuyển quyền giữ library vào storage sống lâu dài.
                     libraries_.push_back(std::move(library));
                     behavior->initialize(plugin.name, nullptr, &global_path_, nullptr, nullptr);
                     creators_.push_back(behavior);
              }
              catch (const std::exception& e)
              {
                     robot::log_error("Failed to load recovery behavior '%s': %s", plugin.type.c_str(), e.what());
                     return;
              }
       //     expect(static_cast<bool>(behavior), "Failed to load plugin: " + plugin.name + " of type: " + plugin.type);
       }
}

void testRotatePlugin()
{
for(const auto& behavior : creators_)
 {
     if(behavior->getNameRecoveryBehavior() == "rotation_rc")
     {
         const recovery_core::RecoveryResult first = behavior->computeCommand(0.1);

         expect(first.status == recovery_core::RecoveryStatus::kRunning,
                "rotate first cycle must be running");
         expect(first.output_type == recovery_core::RecoveryOutputType::kVelocity,
                "rotate must return velocity output");
         expect(first.command.angular.z > 0.0, "rotate must command positive angular.z by default");

         recovery_core::RecoveryResult last = first;
         for (int i = 0; i < 100 && last.status == recovery_core::RecoveryStatus::kRunning; ++i)
         {
           last = behavior->computeCommand(0.1);
         }

         expect(last.status == recovery_core::RecoveryStatus::kSucceeded,
                "rotate must finish within bounded cycles");
         expect(last.output_type == recovery_core::RecoveryOutputType::kVelocity,
                "rotate final result must still be a velocity output");
         expect(std::abs(last.command.angular.z) < 1e-9,
                "rotate must return a zero angular command when complete");
     }
 }
}

void testRegenPathPlugin()
{
robot_geometry_msgs::PoseStamped pose1;
pose1.pose.position.x = 1.0;
robot_geometry_msgs::PoseStamped pose2;
pose2.pose.position.x = 2.0;
global_path_.push_back(pose1);
global_path_.push_back(pose2);


 for(const auto& behavior : creators_)
 {
     if(behavior->getNameRecoveryBehavior() == "regen_path_rc")
     {
         const recovery_core::RecoveryResult result = behavior->runBehavior();

         expect(result.status == recovery_core::RecoveryStatus::kSucceeded,
                "regen path must succeed with a non-empty global path");
         expect(result.output_type == recovery_core::RecoveryOutputType::kPath,
                "regen path must return path output");
         expect(result.path.poses.size() == global_path_.size(),
                "regen path must preserve the global path size");
       global_path_ = result.path.poses;
     }
 }
}

}  // namespace

int main()
{
  testLoadPlugins();
  if(creators_.empty()) return 0;
//   for(auto& behavior : creators_)
//   {
//     std::cout<<behavior->getNameRecoveryBehavior()<<std::endl;
//   }

  testRegenPathPlugin();
  testRotatePlugin();

  std::cout << "[PASS] recovery_core plugin loader contract" << std::endl;
  return 0;
}