#include "component.hpp"
#include <rclcpp/rclcpp.hpp>

namespace component_state_monitor
{
    Component::Component() {}

    Component::Component(const std::string& frame_name): _frame_name(frame_name) {}

    void Component::updateState(const ComponentState& state)
    {
        _current_state = state;
    }

    ComponentState Component::getCurrentState() const 
    {
        return _current_state;
    }

    std::string Component::getFrameName() const 
    {
        return _frame_name;
    }

    ComponentType Component::getType() const
    {
        return _component_type;
    }

    std::vector<std::string> Component::getComponentLinks() const
    {
        return _component_links;
    }

    scene_monitor_interfaces::msg::GraspPose Component::getGraspPose(const std::string &grasp_pose_name) const
    {
        if (_grasp_poses.find(grasp_pose_name) == _grasp_poses.end())
        {
            RCLCPP_WARN(rclcpp::get_logger("component"), "GraspPose with name (%s) not found", grasp_pose_name.c_str());
            return scene_monitor_interfaces::msg::GraspPose();
        }

        return _grasp_poses.at(grasp_pose_name);
    }

    std::vector<scene_monitor_interfaces::msg::GraspPose> Component::getGraspPoses() const
    {
        std::vector<scene_monitor_interfaces::msg::GraspPose> grasp_poses;
        for (auto &pose: _grasp_poses)
        {
            grasp_poses.push_back(pose.second);
        }

        return grasp_poses;
    }

    void Component::setFrameName(const std::string &frame_name)
    {
        _frame_name = frame_name;
    }

    void Component::setGraspPoses(const std::vector<scene_monitor_interfaces::msg::GraspPose> &grasp_poses)
    {
        _grasp_poses.clear();

        for(const auto grasp_pose: grasp_poses)
        {
            _grasp_poses.insert({grasp_pose.label, grasp_pose});
        }
    }

    void Component::setPlacementPose(const geometry_msgs::msg::Pose &pose)
    {
        _placement = pose;
    }

    void Component::setIsImage(const bool &is_image)
    {
        _is_image = is_image;
    }

    bool Component::getIsImage() const
    {
        return _is_image;
    }

    geometry_msgs::msg::Pose Component::getPlacementPose() const
    {
        return _placement;
    }

    inline geometry_msgs::msg::Point construct_point(const json& point)
    {
        geometry_msgs::msg::Point result_point;

        result_point.x = point.at("x");
        result_point.y = point.at("y");
        result_point.z = point.at("z");

        return result_point;
    }

    inline geometry_msgs::msg::Quaternion construct_quaternion(const json& quaternion)
    {
        geometry_msgs::msg::Quaternion result_quaternion;

        result_quaternion.x = quaternion.at("x");
        result_quaternion.y = quaternion.at("y");
        result_quaternion.z = quaternion.at("z");
        result_quaternion.w = quaternion.at("w");

        return result_quaternion;
    }

    inline geometry_msgs::msg::Pose construct_pose(const json& pose)
    {
        geometry_msgs::msg::Pose result_pose;

        result_pose.position = construct_point(pose.at("position"));
        result_pose.orientation = construct_quaternion(pose.at("orientation"));

        return result_pose;
    }

    void Component::initializeFromJson(json &input)
    {
        if (input.empty())
        {
            RCLCPP_WARN(rclcpp::get_logger("component"), "Provided empty json for initialize Component");
            return; 
        }

        std::string type = input.at("type");
        _component_type = type == "part"? ComponentType::Part: ComponentType::Assembly;

        size_t size = input.at("include").size();
        if (size > 0)
        {
            for (auto it=input.at("include").begin(); it != input.at("include").end(); ++it)
            {
                _component_links.push_back(it.value());
            }
        }

        _placement = construct_pose(input.at("features").at("placements").at("placement"));

        auto grasp_poses = input.at("features").at("grasp-poses");
        for (auto it = grasp_poses.begin(); it != grasp_poses.end(); ++it)
        {
            scene_monitor_interfaces::msg::GraspPose grasp_pose;
            grasp_pose.label = it.key();
            grasp_pose.grasp_pose = construct_pose(it.value().at("placement"));
            
            geometry_msgs::msg::Pose pre_pose;
            pre_pose.position.z -= 0.05;

            grasp_pose.pre_grasp_pose = pre_pose;

            grasp_pose.gap_distance = it.value().at("distance");

            _grasp_poses.insert({grasp_pose.label, grasp_pose});
        }

        _current_state = ComponentState::Initialized;
        _is_image = false;
    }

}