#ifndef MOTOR_H
#define MOTOR_H

#include <thread>
#include <memory>

#include <geometry_msgs/msg/twist.hpp>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

#include "irobot_create_msgs/action/drive_distance.hpp"
#include "irobot_create_msgs/action/rotate_angle.hpp"

class DriveDistNode : public rclcpp::Node
{
public:
    DriveDistNode();
    void drive_dist(float meters, float velocity);
    void kill();
private:
    void result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::DriveDistance>::WrappedResult & result);
    rclcpp_action::Client<irobot_create_msgs::action::DriveDistance>::SharedPtr drive_dist_action_;
    bool processing;
};

class SetSpeedNode : public rclcpp::Node
{
public:
    SetSpeedNode();
    void drive(float speed);
    void stop();
    void kill();
private:
    void set_speed(float speed);
    void drive_loop(float speed);
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr speed_publisher_;
    bool run = true;
    std::thread t;
};

class RotateAngleNode : public rclcpp::Node
{
public:
    RotateAngleNode();
    void rotate_angle(float angle, float velocity);
    void kill();
private:
    void result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::RotateAngle>::WrappedResult & result);
    rclcpp_action::Client<irobot_create_msgs::action::RotateAngle>::SharedPtr rotate_angle_action_;
    bool processing;
};

#endif