#include <thread>
#include <memory>
#include <chrono>
#include <mutex>

#include "motor.h"
#include "controls.h"

std::mutex action_mutex;

// #lazyness
void run_node(std::shared_ptr<DriveDistNode> node)
{
    rclcpp::spin(node);
}

void run_node1(std::shared_ptr<SetSpeedNode> node)
{
    rclcpp::spin(node);
}

void run_node2(std::shared_ptr<RotateAngleNode> node)
{
    rclcpp::spin(node);
}

void run_node3(std::shared_ptr<DriveArcNode> node)
{
    rclcpp::spin(node);
}

void run_node4(std::shared_ptr<ButtonPressNode> node)
{
    rclcpp::spin(node);
}

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

    auto ddn = std::make_shared<DriveDistNode>();
    auto ssn = std::make_shared<SetSpeedNode>();
    auto ran = std::make_shared<RotateAngleNode>();
    auto dan = std::make_shared<DriveArcNode>();
    auto bpn = std::make_shared<ButtonPressNode>();
    
    std::thread t;
    std::thread t1;
    std::thread t2;
    std::thread t3;
    std::thread t4;

    t = std::thread(run_node, ddn);
    t1 = std::thread(run_node1, ssn);
    t2 = std::thread(run_node2, ran);
    t3 = std::thread(run_node3, dan);
    t4 = std::thread(run_node4, bpn);

    bpn->bt1_wait();
    bpn->bt2_wait();
    bpn->bt1_wait();

    ssn->drive(0.3);

    std::this_thread::sleep_for (std::chrono::milliseconds(2000));

    ssn->stop();

    // std::this_thread::sleep_for (std::chrono::milliseconds(1000));

    ran->rotate_angle(-45, 0.5);
    // std::this_thread::sleep_for (std::chrono::milliseconds(5000));

    ddn->drive_dist(0.2, 0.3);

    // std::this_thread::sleep_for (std::chrono::milliseconds(5000));

    dan->drive_arc(90, 0.5, 0.5);

    ddn->kill();
    ssn->kill();
    ddn->kill();
    dan->kill();
    bpn->kill();

    t.join();
    t1.join();
    t2.join();
    t3.join();
    t4.join();

    return 0;
}