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

#include "../Extensions.hpp"
#include "ActorMovement.h"
#include <action_msgs/msg/detail/goal_status__struct.hpp>
#include <behaviortree_cpp_v3/basic_types.h>


ActorMovement::ActorMovement(const std::string &name, const BT::NodeConfiguration &config, std::shared_ptr<rclcpp::Node> node, const std::string &actionName, std::function<void(std::shared_ptr<const Movement::Feedback>)> positionCallback)
        : BT::StatefulActionNode(name, config) {
            client = rclcpp_action::create_client<Movement>(node,actionName);
            this->positionCallback = positionCallback;
        }

BT::PortsList ActorMovement::providedPorts() {
    return {
        BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target"),
        BT::InputPort<std::string>("animation_name"),
        BT::InputPort<float>("animation_distance"),
    };
}

BT::NodeStatus ActorMovement::onStart() {
    std::cout << "started actor movement" << std::endl;

    ros_actor_action_server_msgs::action::Movement::Goal goal;

    {
        std::shared_ptr<geometry_msgs::msg::Pose> target;
        if (!getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target", target)) {
            std::cout << "[ no target given ]" << std::endl;
            return BT::NodeStatus::FAILURE;
        }
        goal.target = *target;
    }
    
    if(!getInput<float>("animation_distance", goal.animation_distance)){
        goal.animation_distance = 1.0f;
    }
    
    if(!getInput<std::string>("animation_name", goal.animation_name)){
        std::cout << "[ no animation_name given ]" << std::endl;
        return BT::NodeStatus::FAILURE;
    }

    auto send_goal_options = Client<Movement>::SendGoalOptions();
    send_goal_options.feedback_callback = [&](__attribute__((unused)) std::shared_ptr<ClientGoalHandle<Movement>> goal_handle, std::shared_ptr<const Movement::Feedback> feedback) {
        positionCallback(feedback);
    };
    send_goal_options.result_callback = [&](ClientGoalHandle<Movement>::WrappedResult parameter) {
        mutex.lock();
        if(parameter.code == ResultCode::SUCCEEDED){
            std::cout << "finished actor movement" << std::endl;
            result = BT::NodeStatus::SUCCESS;
        }else{
            std::cout << "failed actor movement" << std::endl;
            result = BT::NodeStatus::FAILURE;
        }
        mutex.unlock();
    };
    send_goal_options.goal_response_callback = [&](ClientGoalHandle<Movement>::SharedPtr parameter){
        mutex.lock();
        if(!parameter){
            std::cout << "ActorMovement rejected by server." << std::endl;
            result = BT::NodeStatus::FAILURE;
        }else{
            if (parameter->get_status() == action_msgs::msg::GoalStatus::STATUS_CANCELED || parameter->get_status() == action_msgs::msg::GoalStatus::STATUS_CANCELING ){
                result = BT::NodeStatus::FAILURE;
            }
        }
        mutex.unlock();
    };

    client->async_send_goal(goal,send_goal_options);
    
    result = BT::NodeStatus::RUNNING;
    return BT::NodeStatus::RUNNING;
}

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

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