#include <control_msgs/action/detail/follow_joint_trajectory__struct.hpp>
#include <memory>
#include <rclcpp_action/client_goal_handle.hpp>
#include <rclcpp_action/types.hpp>
#include <string>
#include <vector>

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>

#include <control_msgs/action/follow_joint_trajectory.hpp>

std::shared_ptr<rclcpp::Node> node;

void common_goal_response(const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr& future){
    RCLCPP_DEBUG(
        node->get_logger(), "common_goal_response time: %f",
        rclcpp::Clock().now().seconds());
    auto goal_handle = future.get();
    if (!goal_handle) {
        printf("Goal rejected\n");
    } else {
        printf("Goal accepted\n");
    }
}

void common_result_response(const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::WrappedResult & result){
    printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds());
    switch (result.code) {
        case rclcpp_action::ResultCode::SUCCEEDED:
            printf("SUCCEEDED result code\n");
            break;
        case rclcpp_action::ResultCode::ABORTED:
            printf("Goal was aborted\n");
            return;
        case rclcpp_action::ResultCode::CANCELED:
            printf("Goal was canceled\n");
            return;
        default:
            printf("Unknown result code\n");
            return;
    }
}

void common_feedback(const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr&,
        const std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Feedback>& feedback){
    std::cout << "feedback->desired.positions :";
    for (auto & x : feedback->desired.positions) {
        std::cout << x << "\t";
    }
    std::cout << std::endl;
    std::cout << "feedback->desired.velocities :";
    for (auto & x : feedback->desired.velocities) {
        std::cout << x << "\t";
    }
    std::cout << std::endl;
}

int main(int argc, char * argv[]){
    rclcpp::init(argc, argv);

    node = std::make_shared<rclcpp::Node>("trajectory_test_node");

    std::cout << "node created" << std::endl;

    rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr action_client;
    action_client = rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(
        node->get_node_base_interface(),
        node->get_node_graph_interface(),
        node->get_node_logging_interface(),
        node->get_node_waitables_interface(),
        "/arm_controller/follow_joint_trajectory"
    );

    bool response = action_client->wait_for_action_server(std::chrono::seconds(10));
    if (!response) {
        throw std::runtime_error("could not get action server");
    }
    std::cout << "Created action server" << std::endl;

    std::vector<std::string> joint_names = {
        "base_rot",
        "base_link1_joint",
        "link1_link2_joint",
        "link2_rot",
        "link2_link3_joint",
        "link3_rot"
    };

    std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
    trajectory_msgs::msg::JointTrajectoryPoint point;
    point.time_from_start = rclcpp::Duration::from_seconds(0.0);  // start asap
    point.positions.resize(joint_names.size());

    point.positions[0] = 0.0;
    point.positions[1] = 0.0;
    point.positions[2] = 0.0;
    point.positions[3] = 0.0;
    point.positions[4] = 0.0;
    point.positions[5] = 0.0;

    trajectory_msgs::msg::JointTrajectoryPoint point2;
    point2.time_from_start = rclcpp::Duration::from_seconds(1.0);
    point2.positions.resize(joint_names.size());

    point2.positions[0] = -1.0;
    point2.positions[1] = 0.0;
    point2.positions[2] = 0.0;
    point2.positions[3] = 0.0;
    point2.positions[4] = 0.0;
    point2.positions[5] = 0.0;

    trajectory_msgs::msg::JointTrajectoryPoint point3;
    point3.time_from_start = rclcpp::Duration::from_seconds(2.0);
    point3.positions.resize(joint_names.size());

    point3.positions[0] = 1.0;
    point3.positions[1] = 0.0;
    point3.positions[2] = 0.0;
    point3.positions[3] = 0.0;
    point3.positions[4] = 0.0;
    point3.positions[5] = 0.0;

    trajectory_msgs::msg::JointTrajectoryPoint point4;
    point4.time_from_start = rclcpp::Duration::from_seconds(3.0);
    point4.positions.resize(joint_names.size());

    point4.positions[0] = 0.0;
    point4.positions[1] = 0.0;
    point4.positions[2] = M_PI/2;
    point4.positions[3] = 0.0;
    point4.positions[4] = 0.0;
    point4.positions[5] = 0.0;

    points.push_back(point);
    points.push_back(point2);
    points.push_back(point3);
    points.push_back(point4);

    rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions opt;
    opt.goal_response_callback = [](std::shared_ptr<rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>> goal){ 
        if(goal->get_status()==rclcpp_action::GoalStatus::STATUS_ACCEPTED){
            RCLCPP_DEBUG(
                node->get_logger(), "common_goal_response accepted: %f",
                rclcpp::Clock().now().seconds());
        }else{
            RCLCPP_DEBUG(
                node->get_logger(), "common_goal_response rejected: %f",
                rclcpp::Clock().now().seconds());
        }
    };
    //opt.goal_response_callback = [](auto && PH1) { return common_goal_response(std::forward<decltype(PH1)>(PH1)); };
    opt.result_callback = [](auto && PH1) { return common_result_response(std::forward<decltype(PH1)>(PH1)); };
    opt.feedback_callback = [](auto && PH1, auto && PH2) { return common_feedback(std::forward<decltype(PH1)>(PH1), std::forward<decltype(PH2)>(PH2)); };

    control_msgs::action::FollowJointTrajectory_Goal goal_msg;
    goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0);
    goal_msg.trajectory.joint_names = joint_names;
    goal_msg.trajectory.points = points;

    auto goal_handle_future = action_client->async_send_goal(goal_msg, opt);

    if (rclcpp::spin_until_future_complete(node, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS){
        RCLCPP_ERROR(node->get_logger(), "send goal call failed :(");
        return 1;
    }
    RCLCPP_ERROR(node->get_logger(), "send goal call ok :)");

    const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr& goal_handle = goal_handle_future.get();
    if (!goal_handle) {
        RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server");
        return 1;
    }
    RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server");

    // Wait for the server to be done with the goal
    auto result_future = action_client->async_get_result(goal_handle);
    RCLCPP_INFO(node->get_logger(), "Waiting for result");
    if (rclcpp::spin_until_future_complete(node, result_future) != rclcpp::FutureReturnCode::SUCCESS){
        RCLCPP_ERROR(node->get_logger(), "get result call failed :(");
        return 1;
    }

    std::cout << "async_send_goal" << std::endl;
  
    rclcpp::shutdown();

    return 0;
}