//
// Created by bastian on 26.03.22.
//

#include "RobotMove.h"

BT::RobotMove::RobotMove(const std::string &name, const BT::NodeConfiguration &config,
                         std::shared_ptr<MoveConnection> connection)
        : StatefulActionNode(name, config) {
    this->connection = connection;
}

BT::NodeStatus BT::RobotMove::onStart() {
    std::cout << "Starting RobotMove" << std::endl;
    std::shared_ptr<geometry_msgs::msg::Pose> pose;
    if(!getInput("target",pose)){
        std::cout << "no target available." << std::endl;
        return NodeStatus::FAILURE;
    }
    printf("X:%f Y:%f Z:%f\n",pose->position.x,pose->position.y,pose->position.z);
    printf("W:%f X:%f Y:%f Z:%f\n",pose->orientation.w,pose->orientation.x,pose->orientation.y,pose->orientation.z);
    asyncResult = NodeStatus::RUNNING;
    connection->planAndExecute(pose,[this](bool finished){
        if(finished){
            printf("Finished move.");
            asyncResult = NodeStatus::SUCCESS;
        } else {
            printf("Failed move.");
            asyncResult = NodeStatus::FAILURE;
        }
    });
    return NodeStatus::RUNNING;
}

BT::NodeStatus BT::RobotMove::onRunning() {
    auto result = asyncResult;
    if(asyncResult!=NodeStatus::RUNNING){
        asyncResult=NodeStatus::IDLE;
    }
    return result;
}

void BT::RobotMove::onHalted() {
    std::cout << "Halting RobotMove" << std::endl;
    connection->stop();
    asyncResult=NodeStatus::FAILURE;
}

BT::PortsList BT::RobotMove::providedPorts() {
    return {InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target", "pose target for robot")};
}