//
// Created by AI Assistant
//

#pragma once

#include <memory>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_model/joint_model_group.h>
#include <manipulation_planning/scene_interface/planning_scene_interface.hpp>

namespace ims {

/**
 * @brief Concrete implementation of PlanningSceneInterface that wraps MoveIt's PlanningScene
 */
class MoveItPlanningSceneWrapper : public PlanningSceneInterface {
private:
    std::shared_ptr<planning_scene::PlanningScene> moveit_planning_scene_;

public:
    explicit MoveItPlanningSceneWrapper(std::shared_ptr<planning_scene::PlanningScene> planning_scene) 
        : moveit_planning_scene_(planning_scene) {}

    virtual ~MoveItPlanningSceneWrapper() = default;

    void* getMoveItPlanningScene() const override {
        return static_cast<void*>(moveit_planning_scene_.get());
    }
    
    void* getRobotModel() const override {
        return const_cast<void*>(static_cast<const void*>(moveit_planning_scene_->getRobotModel().get()));
    }
    
    void* getJointModelGroup(const std::string& group_name) const override {
        return const_cast<void*>(static_cast<const void*>(moveit_planning_scene_->getRobotModel()->getJointModelGroup(group_name)));
    }
    
    /**
     * @brief Get the MoveIt planning scene as a shared_ptr
     * This is a convenience method for the srmp_moveit_plugin
     */
    std::shared_ptr<planning_scene::PlanningScene> getMoveItPlanningSceneShared() const {
        return moveit_planning_scene_;
    }
};

} // namespace ims
