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

#include "ActorAnimation.h"


ActorAnimation::ActorAnimation(const std::string &name, const BT::NodeConfiguration &config, std::shared_ptr<rclcpp::Node> node, const std::string &actionName)
        : BT::StatefulActionNode(name, config) {
            client = rclcpp_action::create_client<Animation>(node,actionName);
        }

BT::PortsList ActorAnimation::providedPorts() {
    return {
        BT::InputPort<std::string>("animation_name"),
        BT::InputPort<float>("animation_speed"),
    };
}

BT::NodeStatus ActorAnimation::onStart() {
    std::cout << "started actor animation" << std::endl;

    ros_actor_action_server_msgs::action::Animation::Goal goal;

    if (!getInput<std::string>("animation_name", goal.animation_name)) {
        std::cerr << "[ no animation name specified ]" << std::endl;
        return BT::NodeStatus::FAILURE;
    }
    
    if (!getInput<float>("animation_speed", goal.animation_speed)) {
        goal.animation_speed=1.0;
    }

    

    auto send_goal_options = Client<Animation>::SendGoalOptions();
    send_goal_options.result_callback = [=](const ClientGoalHandle<Animation>::WrappedResult & parameter) {
        mutex.lock();
        if(parameter.code == ResultCode::SUCCEEDED){
            std::cout << "finished actor animation" << std::endl;
            result = BT::NodeStatus::SUCCESS;
        }else{
            std::cout << "failed actor animation" << std::endl;
            result = BT::NodeStatus::FAILURE;
        }
        mutex.unlock();
    };

    client->async_send_goal(goal,send_goal_options);

    result = BT::NodeStatus::RUNNING;
    return BT::NodeStatus::RUNNING;
}

BT::NodeStatus ActorAnimation::onRunning() {
    mutex.lock();
    auto status = result;
    mutex.unlock();
    return status;
}

void ActorAnimation::onHalted() {
    std::cout << "halted actor animation" << std::endl;
    client->async_cancel_all_goals();
    result = BT::NodeStatus::FAILURE;
}
