//
// Created by bastian on 29.03.22.
//

#include "../Extensions.hpp"
#include "OffsetPose.h"
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <memory>


BT::PortsList BT::OffsetPose::providedPorts() {
    return {
            InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("input", "initial position as Pose"),
            InputPort<std::shared_ptr<geometry_msgs::msg::Point>>("offset", "offset as a Point"),
            InputPort<std::shared_ptr<geometry_msgs::msg::Quaternion>>("orientation", "rotation of resulting pose as Quaternion"),
            OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("output", "generated new pose")
    };
}

BT::OffsetPose::OffsetPose(const std::string &name,
        const BT::NodeConfiguration &config) : SyncActionNode(name, config) {}

BT::NodeStatus BT::OffsetPose::tick() {
    
    std::shared_ptr<geometry_msgs::msg::Pose> pose;
    if (!getInput("input", pose)) {
        std::cout << "Missing {input} pose" << std::endl;
        return NodeStatus::FAILURE;
    }
    
    auto resultPose = std::make_shared<geometry_msgs::msg::Pose>(*pose);

    std::shared_ptr<geometry_msgs::msg::Quaternion> quaternion;
    if(getInput("orientation", quaternion)) {
        resultPose->orientation.w = quaternion->w;
        resultPose->orientation.x = quaternion->x;
        resultPose->orientation.y = quaternion->y;
        resultPose->orientation.z = quaternion->z;
    }

    std::shared_ptr<geometry_msgs::msg::Point> offset;
    if(getInput("offset", offset)) {
        resultPose->position.x += offset->x;
        resultPose->position.y += offset->y;
        resultPose->position.z += offset->z;
    }

    setOutput("output", resultPose);

    return BT::NodeStatus::SUCCESS;
}
