#include "nova5_control/imr_nova_control.h"
#include <iostream>
#include <memory>
#include <ros/ros.h>
#include <std_msgs/UInt16.h>

std::shared_ptr<imr_nova_control> controller = nullptr;

unsigned int ok_count_max = 0;
unsigned int ng_count_max = 1;
bool enable = true;
bool continue_flag = false;
bool go_home_flag = false;
bool power_on_flag = false;

void callBack(const std_msgs::UInt16::ConstPtr& msg)
{
    if(controller != nullptr)
    {
        switch (msg->data)
        {
        case 1:
        controller->startHomeThread();
            // continue_flag = true;
            // go_home_flag = false;
            // power_on_flag = false;
            break;
        case 2:
            // continue_flag = false;
            // go_home_flag = true;
            // power_on_flag = false;
            break;
        case 3:
            // continue_flag = false;
            // go_home_flag = false;
            // power_on_flag = true;
            break;        
        default:
            // continue_flag = false;
            // go_home_flag = false;
            // power_on_flag = false;
            break;
        }
    }
}

int main(int argc, char** argv) {
    // Initialize the ROS node
    ros::init(argc, argv, "test_imr_nova_control");
    ros::NodeHandle nh = ros::NodeHandle("~");
    ros::Subscriber sub = nh.subscribe("mode", 1, callBack);
    ros::Publisher pub = nh.advertise<std_msgs::UInt16>("status", 1);

    try {
        // Initialize ROS time
        // ros::Time::init();

        // Create an instance of imr_nova_control
        controller = std::make_shared<imr_nova_control>();

        

        controller->ok_count_max_ = &ok_count_max;
        controller->ng_count_max_ = &ng_count_max;
        controller->enable_ = &enable;

        controller->continue_flag_ = &continue_flag;
        controller->go_home_flag_ = &go_home_flag;
        controller->power_on_flag_ = &power_on_flag;

        
        #if 0
        controller->startHomeThread();
        #endif

        #if 0
        controller->startModeThread();
        #endif

        std::cout << "imr_nova_control instance created successfully." << std::endl;

        // Run ROS spin to process callbacks (if applicable)
        // ros::AsyncSpinner spinner(2); // Adjust the number of threads as needed
        // spinner.start();
        ros::Rate rate(5);

        while (ros::ok())
        {
            std_msgs::UInt16 msg;
            msg.data = controller->statusCode_;
            pub.publish(msg);
            ros::spinOnce();
            rate.sleep();
        }

    } catch (const std::exception& e) {
        std::cerr << "An error occurred: " << e.what() << std::endl;
    }
    ros::spin();
    return 0;
}