#pragma once

#include <string>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/pose.hpp>

#include "scene_monitor_interfaces/msg/grasp_pose.hpp"
#include "nlohmann_json/json.hpp"

namespace component_state_monitor
{

using json = nlohmann::json;

/* 
Description:
    Initialized: 
        - state when model is initialized by provided metadata
    Printed:
        - model is ready for manipulation and other operations in sim/real
    Posted:
        - model is posted with another
*/

enum ComponentState
{
    Initialized,
    Printed,
    Posted
};

enum ComponentType
{
    Part,
    Assembly
};

class Component
{
public:

    Component();
    Component(const std::string &frame_name);

    ComponentState getCurrentState() const;
    ComponentType getType() const;
    std::vector<std::string> getComponentLinks() const;
    std::string getFrameName() const;
    scene_monitor_interfaces::msg::GraspPose getGraspPose(const std::string &grasp_pose_name) const;
    std::vector<scene_monitor_interfaces::msg::GraspPose> getGraspPoses() const;
    geometry_msgs::msg::Pose getPlacementPose() const;

    void setFrameName(const std::string &frame_name);
    
    void initializeFromJson(json &input);

    void updateState(const ComponentState &state);

private:

    std::string _frame_name;
    ComponentState _current_state;
    ComponentType _component_type;
    geometry_msgs::msg::Pose _placement;
    std::map<std::string, scene_monitor_interfaces::msg::GraspPose> _grasp_poses;
    std::vector<std::string> _component_links;

    geometry_msgs::msg::Pose _current_pose;
};

}