#ifndef    __UDEVADM_INFO_H_INCLUDE__
#define    __UDEVADM_INFO_H_INCLUDE__

#include <ros/ros.h>
#include <unistd.h>
#include <poll.h>
#include <cstdio>
#include <cstdlib>
#include <cerrno>
#include <cstring>
#include <sys/signalfd.h>
#include <csignal>
#include <libudev.h>
#include <string>

class udevadmInfo {
public:
    udevadmInfo(const char *product_name);
    virtual ~udevadmInfo(void);
    virtual int init();
    virtual bool scanDevices(void);
    const char *port;
protected:
    struct udev *udev;
    char *product_name_;
};

udevadmInfo::udevadmInfo(const char *product_name)
 : product_name_((char *)product_name)
{
    
}

udevadmInfo::~udevadmInfo(void){
    udev_unref(udev);
}

int udevadmInfo::init() {
    udev = udev_new();
    if (!udev) {
        printf("Error while initialization!\n");
        return EXIT_FAILURE;
    }
    sigset_t mask;

    // Set signals we want to catch
    sigemptyset(&mask);
    sigaddset(&mask, SIGTERM);
    sigaddset(&mask, SIGINT);

    // Change the signal mask and check
    if (sigprocmask(SIG_BLOCK, &mask, nullptr) < 0) {
        ROS_ERROR("UDEV-Error while sigprocmask(): %s\n", std::strerror(errno));
        return EXIT_FAILURE;
    }
    // Get a signal file descriptor
    int signal_fd = signalfd(-1, &mask, 0);
    // Check the signal file descriptor
    if (signal_fd < 0) {
        ROS_ERROR("UDEV-Error while signalfd(): %s\n", std::strerror(errno));
        return EXIT_FAILURE;
    }
    return EXIT_SUCCESS;
}

bool udevadmInfo::scanDevices(void) {
    struct udev_device *device;
    struct udev_enumerate *enumerate;
    struct udev_list_entry *devices, *dev_list_entry;

    // Create enumerate object
    enumerate = udev_enumerate_new(udev);
    if (!enumerate) {
        ROS_ERROR("Error while creating udev enumerate\n");
        return false;
    }

    // Scan devices
    udev_enumerate_scan_devices(enumerate);

    // Fill up device list
    devices = udev_enumerate_get_list_entry(enumerate);
    if (!devices) {
        ROS_ERROR("Error while getting device list\n");
        return false;
    }

    udev_list_entry_foreach(dev_list_entry, devices) {
        const char* path = udev_list_entry_get_name(dev_list_entry);
        // Get the device
        device = udev_device_new_from_syspath(udev, udev_list_entry_get_name(dev_list_entry));
        // Print device information
        if(udev_device_get_devnode(device) != NULL && strstr(udev_device_get_sysname(device),"ttyUSB") != NULL) {
            // ROS_INFO("DEVNODE=%s", udev_device_get_devnode(device));
            struct udev_device* usb = udev_device_get_parent_with_subsystem_devtype(device,"usb","usb_device");
                // ROS_INFO("usb = %s:%s, manufacturer = %s, product = %s, serial = %s, speed = %s, bMaxPower = %s",
                //         udev_device_get_sysattr_value(usb, "idVendor"),
                //         udev_device_get_sysattr_value(usb, "idProduct"),
                //         udev_device_get_sysattr_value(usb, "manufacturer"),
                //         udev_device_get_sysattr_value(usb, "product"),
                //         udev_device_get_sysattr_value(usb, "serial"),
                //         udev_device_get_sysattr_value(usb, "speed"),
                //         udev_device_get_sysattr_value(usb, "bMaxPower"));
                if(strstr(udev_device_get_sysattr_value(usb, "product"), (const char *)product_name_) != NULL) {
                    port = udev_device_get_devnode(device);
                    return true;
                }
        }
        else {
            continue;
        }
        // Free the device
        udev_device_unref(device);
    }
    // Free enumerate
    udev_enumerate_unref(enumerate);
    return false;
}
#endif