#include <chrono>
#include <thread>
#include <mutex>
#include <future>
#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"

#include "motor.h"
#include "sequence_lock.h"

double pi = 2 * acos(0.0);

DriveDistNode::DriveDistNode()
: Node("drive_dist_node")
{
    drive_dist_action_ = rclcpp_action::create_client<irobot_create_msgs::action::DriveDistance>(
        this,
        "/drive_distance"
    );
}

void DriveDistNode::drive_dist(float meters, float velocity)
{
    std::cout << "drive dist" << std::endl;
    processing = true;
    auto data = irobot_create_msgs::action::DriveDistance::Goal();
    auto send_goal_options = rclcpp_action::Client<irobot_create_msgs::action::DriveDistance>::SendGoalOptions();
    send_goal_options.result_callback =
        std::bind(&DriveDistNode::result_callback, this, std::placeholders::_1);

    data.distance = meters;
    data.max_translation_speed = velocity;
    drive_dist_action_->async_send_goal(data, send_goal_options);

    while (processing) {}
}

void DriveDistNode::result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::DriveDistance>::WrappedResult & result)
{
    processing = false;
    std::cout << "finished dist" << std::endl;
    switch (result.code) {
        case rclcpp_action::ResultCode::SUCCEEDED:
            return;
        case rclcpp_action::ResultCode::ABORTED:
            RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
            return;
        case rclcpp_action::ResultCode::CANCELED:
            RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
            return;
        default:
            RCLCPP_ERROR(this->get_logger(), "Unknown result code");
            return;
    }
}

void DriveDistNode::kill()
{
    std::cout << "DriveDistNode killed" << std::endl;
    rclcpp::shutdown();
}

SetSpeedNode::SetSpeedNode()
: Node("set_speed_node")
{
    speed_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>(
        "/cmd_vel",
        rclcpp::SensorDataQoS()
    );
}

void SetSpeedNode::drive_loop(float speed)
{
    std::cout << "Loop" << std::endl;
    while (run)
    {
        set_speed(speed);
        // sleep set as described at http://wiki.ros.org/Robots/TIAGo/Tutorials/motions/cmd_vel
        std::this_thread::sleep_for (std::chrono::milliseconds(333));
    }
}

void SetSpeedNode::set_speed(float speed)
{
    auto data = geometry_msgs::msg::Twist();

    data.linear.x = speed;
    data.linear.y = 0;
    data.linear.z = 0;
    data.angular.x = 0;
    data.angular.x = 0;
    data.angular.x = 0;
    speed_publisher_->publish(data);
}

void SetSpeedNode::drive(float speed)
{
    std::cout << "Drive" << std::endl;
    run = true;
    t = std::thread(&SetSpeedNode::drive_loop, this, speed);
}

void SetSpeedNode::stop()
{
    std::cout << "stop" << std::endl;
    run = false;
}

void SetSpeedNode::kill()
{
    std::cout << "SetSpeedNode killed" << std::endl;
    rclcpp::shutdown();
}

RotateAngleNode::RotateAngleNode()
: Node("rotate_angle_node")
{
    rotate_angle_action_ = rclcpp_action::create_client<irobot_create_msgs::action::RotateAngle>(
        this,
        "rotate_angle"
    );
}

void RotateAngleNode::rotate_angle(float angle, float velocity)
{
    processing = true;
    angle *= pi / 180;
    auto data = irobot_create_msgs::action::RotateAngle::Goal();
    auto send_goal_options = rclcpp_action::Client<irobot_create_msgs::action::RotateAngle>::SendGoalOptions();
    send_goal_options.result_callback =
        std::bind(&RotateAngleNode::result_callback, this, std::placeholders::_1);
    

    data.angle = angle;
    data.max_rotation_speed = velocity;
    rotate_angle_action_->async_send_goal(data, send_goal_options);

    while (processing) {}
}

void RotateAngleNode::result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::RotateAngle>::WrappedResult & result)
{
    processing = false;
    std::cout << "finished rotation" << std::endl;
    switch (result.code) {
        case rclcpp_action::ResultCode::SUCCEEDED:
            return;
        case rclcpp_action::ResultCode::ABORTED:
            RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
            return;
        case rclcpp_action::ResultCode::CANCELED:
            RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
            return;
        default:
            RCLCPP_ERROR(this->get_logger(), "Unknown result code");
            return;
    }
}

void RotateAngleNode::kill()
{
    std::cout << "RotateAngleNode killed" << std::endl;
    rclcpp::shutdown();
}