//
// Created by AI Assistant
//

#pragma once

#include <memory>
#include <moveit/distance_field/propagation_distance_field.h>
#include <manipulation_planning/scene_interface/distance_field_interface.hpp>

namespace ims {

/**
 * @brief Concrete implementation of DistanceFieldInterface that wraps MoveIt's PropagationDistanceField
 */
class MoveItDistanceFieldWrapper : public DistanceFieldInterface {
private:
    std::shared_ptr<distance_field::PropagationDistanceField> moveit_df_;

public:
    explicit MoveItDistanceFieldWrapper(std::shared_ptr<distance_field::PropagationDistanceField> df) 
        : moveit_df_(df) {}

    virtual ~MoveItDistanceFieldWrapper() = default;

    double getSizeX() const override {
        return moveit_df_->getSizeX();
    }
    
    double getSizeY() const override {
        return moveit_df_->getSizeY();
    }
    
    double getSizeZ() const override {
        return moveit_df_->getSizeZ();
    }
    
    double getDistance(double x, double y, double z) const override {
        return moveit_df_->getDistance(x, y, z);
    }
    
    void* getMoveItDistanceField() const override {
        return static_cast<void*>(moveit_df_.get());
    }
    
    /**
     * @brief Get the MoveIt distance field as a shared_ptr
     * This is a convenience method for the srmp_moveit_plugin
     */
    std::shared_ptr<distance_field::PropagationDistanceField> getMoveItDistanceFieldShared() const {
        return moveit_df_;
    }
};

} // namespace ims
