/*  Copyright 2025 Leuze electronic GmbH + Co. KG
 *
 *  Licensed under the Apache License, Version 2.0
 */

#include <chrono>
#include <memory>

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>

#include "leuze_msgs/msg/status_profile_msg_rod.hpp"
#include "leuze_rod_driver/common.h"
#include "leuze_rod_driver/eth_comm_rod.h"
#include "leuze_rod_driver/eth_comm_sim.h"
#include "leuze_rod_driver/rod_node.h"

namespace leuze_rod_ros2_drivers
{
    using namespace std::chrono_literals;

    /************************************************************************************************************
     * @class   RodNode
     * 
     * @brief   This class is the ROS node that ensures the whole ROD driver functionality
     ************************************************************************************************************/

    /**
     * @brief constructor
     */
    RodNode::RodNode(std::string ip_addr, std::string port_num, std::string topic_id, std::string frame_id) : Node("leuze_rod_driver")
    {
        /* save RODxx IP address, used port number, topic ID and frame ID*/
        ip_addr_    = ip_addr;
        port_num_   = std::stoi(port_num);
        topic_id_   = topic_id;
        frame_id_   = frame_id;
        
        /* init other variables */
        eth_comm_ifc_                   = nullptr;
        time_last_rod_cont_warn_log_    = this->now() - rclcpp::Duration(ROD_LOG_SUPPRESS_TIME, 0);
        time_last_rod_cont_error_log_   = this->now() - rclcpp::Duration(ROD_LOG_SUPPRESS_TIME, 0);
        time_last_rod_error_log_        = this->now() - rclcpp::Duration(ROD_LOG_SUPPRESS_TIME, 0);
        error_last_log_                 = 0;

        logInfoDriverStartup();

        #ifdef DEBUG
            logWarnDebug();
        #endif
   
        /* init ethernet communication interface */
        #ifdef SIMULATION
            eth_comm_ifc_ = new EthCommSim();                                                   // interface to communicate with a ROD scanner simulation
            logWarnSimulation();
        #else
            eth_comm_ifc_ = new EthCommRod();                                                   // interface to communicate with a real ROD scanner
            RCLCPP_INFO(this->get_logger(), "Connecting to TCP command interface at %s:%d", ip_addr_.c_str(), port_num_);
        #endif

        if(nullptr == eth_comm_ifc_)
        {
            RCLCPP_FATAL(this->get_logger(), "Unable to create ethernet communication interface!");
            throw std::runtime_error("Unable to create ethernet communication interface!");         // connection with the scanner is crucial => throw exception
        } 
        else
        {
            /* init used variables, structures, etc. */
            count_status_failed_ = 0;
            memset(&laser_scan_params_, 0, sizeof(RodScanParams));

            /* connect to ROD scanner and read its settings and status data */
            if(false == eth_comm_ifc_->connect(ip_addr_, port_num_))                                    // connect to ROD scanner
            {
                RCLCPP_FATAL(this->get_logger(), "Connection with scanner has not been estabilished!");
                throw std::runtime_error("Connection with scanner has not been estabilished!");         // connection with the scanner is crucial => throw exception
            }
            else
            {
                loadConfiguration();
            } 

            time_last_published_scan_ = this->now();                                                    // init variable of last published scan 

            /* scan data publishing */
            scan_publishing_timer_  = this->create_wall_timer(SCANPAR_PUBLISHER_PERIOD, std::bind(&RodNode::publishScanData, this));
            scan_publisher_         = this->create_publisher<sensor_msgs::msg::LaserScan>(topic_id_, rclcpp::QoS(rclcpp::KeepLast(SCANPAR_PUBLISHER_QUE_SIZE)));

            /* status data publishing*/
            #ifdef STATUS 
                status_publishing_timer_    = this->create_wall_timer(STATPAR_PUBLISHER_PERIOD, std::bind(&RodNode::publishStatusData, this));
                status_publisher_           = this->create_publisher<leuze_msgs::msg::StatusProfileMsgRod>(topic_id_ + "_status", rclcpp::QoS(rclcpp::KeepLast(STATPAR_PUBLISHER_QUE_SIZE)));
            #else
                status_publishing_timer_    = nullptr;
                status_publisher_           = nullptr;
            #endif 

            /* final info for driver users */
            logInfoTopicsAndFrame();
        }
    }

    /**
     * @brief deconstructor
     */
    RodNode::~RodNode()
    {
        delete eth_comm_ifc_;
        eth_comm_ifc_ = nullptr;
    } 

    /****************************************************************************************************/

    /**
     * @brief get and publish all available scans (if there are any)
     */
    void RodNode::publishScanData()
    {
        int available_scans = eth_comm_ifc_->getCompleteScansAvailable();

        if(available_scans > 0)
        { 
            for(int i = 0; i < available_scans; i++)
            {
                auto scan_data = eth_comm_ifc_->getCompleteScan();
                int scan_size = scan_data.distances.size();
                
                if(scan_size)
                {
                    auto laser_scan_msg = std::make_shared<sensor_msgs::msg::LaserScan>();

                    /* fill header */
                    laser_scan_msg->header.stamp    = this->now();
                    laser_scan_msg->header.frame_id = frame_id_;
                    
                    /* fill scan parameters */
                    laser_scan_msg->angle_min       = laser_scan_params_.angle_min;
                    laser_scan_msg->angle_max       = laser_scan_params_.angle_max;
                    laser_scan_msg->angle_increment = laser_scan_params_.angle_delta;
                    laser_scan_msg->scan_time       = laser_scan_params_.scan_duration;
                    laser_scan_msg->time_increment  = laser_scan_params_.scan_duration / (double)(scan_size - 1);
                    laser_scan_msg->range_min       = ROD_DEFAULT_RANGE_MIN;
                    laser_scan_msg->range_max       = ROD_DEFAULT_RANGE_MAX;

                    /* fill scan data */
                    switch(laser_scan_params_.packet_type)
                    {
                        case RODPAR_PTYPE_DISTANCES_ONLY:
                            laser_scan_msg->ranges.resize(scan_size);
                            for(int i = 0; i < scan_size; i++)
                            {
                                laser_scan_msg->ranges[i] = float(scan_data.distances.at(i)) / 1000.0f;
                            }
                            break;

                        case RODPAR_PTYPE_DISTANCE_AND_AMPLITUDE:
                            laser_scan_msg->ranges.resize(scan_size);
                            laser_scan_msg->intensities.resize(scan_size);
                            for(int i = 0; i < scan_size; i++)
                            {
                                laser_scan_msg->ranges[i]       = float(scan_data.distances.at(i)) / 1000.0f;
                                laser_scan_msg->intensities[i]  = float(scan_data.intensities.at(i));
                            }
                            break;

                        default:
                            RCLCPP_FATAL(this->get_logger(), "Unable to resolve MDI packet type to publish data!!!");
                            throw std::runtime_error("Unable to resolve MDI packet type to publish data!!!");
                            break;
                    } 

                    scan_publisher_->publish(*laser_scan_msg);                                  // publish scan 

                    time_last_published_scan_ = this->now();
                }
                else
                {
                    continue;
                } 
            }
        }
        else
        {
            if ((time_last_published_scan_.seconds() + SCANPAR_PUBLISH_TIMEOUT_S) > this->now().seconds())
            {
                // OK
            }   
            else
            {
                RCLCPP_FATAL(this->get_logger(), "Unable to publish correct scan data for time period > %d s", SCANPAR_PUBLISH_TIMEOUT_S);
                throw std::runtime_error("Scan data communication is not stable! Stopping the driver!!!");
            } 
        }  
    }

    /**
     * @brief refresh and publish ROD status data
     */
    void RodNode::publishStatusData()
    {
        if (0 == eth_comm_ifc_->refreshStatusData())
        {
            auto status_data        = eth_comm_ifc_->getStatusData();
            auto status_data_msg    = std::make_shared<leuze_msgs::msg::StatusProfileMsgRod>();

            /* Header */
            status_data_msg->header.stamp           = this->now();
            status_data_msg->header.frame_id        = frame_id_;

            /* MDI info */
            status_data_msg->mdi_protocol           = status_data.mdi_protocol;
            status_data_msg->mdi_packet_type        = status_data.mdi_packet_type;
            status_data_msg->mdi_data_direction     = status_data.mdi_data_direction;
            status_data_msg->mdi_tx_status          = status_data.mdi_tx_status;

            /* Scan info */
            status_data_msg->scan_resolution        = status_data.scan_resolution;
            status_data_msg->scan_angle_min         = status_data.scan_angle_min;
            status_data_msg->scan_angle_max         = status_data.scan_angle_max;
            status_data_msg->scan_skip_spots        = status_data.scan_skip_spots;

            /* Contamination info */
            status_data_msg->cont_threshold_warn    = status_data.cont_threshold_warn;
            status_data_msg->cont_threshold_error   = status_data.cont_threshold_error;
            for(int i = 0; i < 9; i++)
            {
                status_data_msg->cont_state[i]      = status_data.cont_state[i];

                if((status_data_msg->cont_state[i] >= status_data_msg->cont_threshold_warn) && (status_data_msg->cont_state[i] < status_data_msg->cont_threshold_error))
                {
                    if(time_last_rod_cont_warn_log_ < this->now() - rclcpp::Duration(ROD_LOG_SUPPRESS_TIME, 0))
                    {
                        RCLCPP_WARN(this->get_logger(), "Laser scanner window is contaminated. Clean it, please.");
                        time_last_rod_cont_warn_log_ = this->now();
                    }
                    else
                    {
                        // log is suppressed
                    }
                }
                else if(status_data_msg->cont_state[i] >= status_data_msg->cont_threshold_error)
                {
                    if(time_last_rod_cont_error_log_ < this->now() - rclcpp::Duration(ROD_LOG_SUPPRESS_TIME, 0))
                    {
                        RCLCPP_ERROR(this->get_logger(), "Laser scanner window is contaminated!!! Clean it, please.");
                        time_last_rod_cont_error_log_ = this->now();
                    }
                    else
                    {
                        // log is suppressed
                    }
                }
                else
                {
                    // OK
                }
            }

            /* ROD scanner info */
            status_data_msg->rod_part_number        = status_data.rod_part_number;
            status_data_msg->rod_hw_ver             = status_data.rod_hw_ver;
            status_data_msg->rod_sw_ver             = status_data.rod_sw_ver;
            status_data_msg->rod_temp               = status_data.rod_temp;
            status_data_msg->rod_hours              = status_data.rod_hours;
            status_data_msg->rod_window_calib       = status_data.rod_window_calib;

            /* Filter */
            status_data_msg->filter_type            = status_data.filter_type;
            status_data_msg->filter_spots_hist      = status_data.filter_spots_hist;
            status_data_msg->filter_spots_nghb      = status_data.filter_spots_nghb;

            /* Errors */
            status_data_msg->error                  = status_data.error;
            if(0 != status_data_msg->error)
            {
                if((time_last_rod_error_log_ < this->now() - rclcpp::Duration(ROD_LOG_SUPPRESS_TIME, 0)) || status_data_msg->error != error_last_log_)
                {
                    RCLCPP_ERROR(this->get_logger(), "Error code %d reported by Laser scanner!!!",  status_data_msg->error);
                    error_last_log_ = status_data_msg->error;
                    time_last_rod_error_log_ = this->now();
                }
                else
                {
                    // log is suppressed
                }
            }
            else
            {
                if(0 != error_last_log_)
                {   
                    RCLCPP_INFO(this->get_logger(), "Currently is no error indicated by Laser scanner.");
                    error_last_log_ = 0;
                }
                else
                {
                    // nothing to log
                }
            }
            for(int i = 0; i < 10; i++)
            {
                status_data_msg->error_log_code[i]  = status_data.error_log_code[i];
                status_data_msg->error_log_date[i]  = status_data.error_log_date[i]; 
            }

            status_publisher_->publish(*status_data_msg);
        }
        else
        {
            rclcpp::Time time_status_failed = this->now();

            if(time_status_failed.seconds() > (time_status_failed_.seconds() + STATPAR_MAX_FAILED_TIMEPERIOD_S))
            {
                RCLCPP_WARN(this->get_logger(), "Unable to publish correct status data!");
                count_status_failed_ = 1;
            }
            else
            {
                if(count_status_failed_<= STATPAR_MAX_FAILED_COUNT)
                {
                    RCLCPP_WARN(this->get_logger(), "Unable to publish correct status data! (Fail counter values is %d)", count_status_failed_);
                    count_status_failed_++;
                } 
                else
                {
                    RCLCPP_FATAL(this->get_logger(), "Unable to publish correct status data (communication has been lost or it is not stable)!");
                    throw std::runtime_error("Status data communication is not stable! Stopping the driver!!!");
                } 
            }  

            time_status_failed_ = this->now();
        }
    } 
   
    /****************************************************************************************************/

    void RodNode::loadConfiguration()
    {
        RCLCPP_INFO(this->get_logger(), "Loading configuration from ROD...");

        if( CMDITF_RETVAL_OK != eth_comm_ifc_->readStatusData())
        {
            RCLCPP_FATAL(this->get_logger(), "... Unable to read status data from scanner!");
            throw std::runtime_error("Unable to read status data from scanner!");
        }
        else
        {
            auto rod_settings = eth_comm_ifc_->getStatusData();                            // get ROD scanner settings and status data, that has been loaded after connection with the scanner 

            /* Scanning Angle Range */
            RCLCPP_INFO(this->get_logger(), "... Scanning Angle Range:");
            if(rod_settings.scan_angle_min >= RODRNG_ANGLE_MIN && rod_settings.scan_angle_min <= RODRNG_ANGLE_MAX)
            {
                RCLCPP_INFO(this->get_logger(), "...... Boundary 1 = %.1f°", float(rod_settings.scan_angle_min) / 100.0f);
            } 
            else
            {
                RCLCPP_FATAL(this->get_logger(), "...... Boundary 1 out of expected range (%d [0.01°])!", rod_settings.scan_angle_min);
                throw std::runtime_error("Scanning Angle Range (Boundary 1) is out of expected range!");
            } 
            if(rod_settings.scan_angle_max >= RODRNG_ANGLE_MIN && rod_settings.scan_angle_max <= RODRNG_ANGLE_MAX)
            { 
                RCLCPP_INFO(this->get_logger(), "...... Boundary 2 = %.1f°", float(rod_settings.scan_angle_max) / 100.0f);
            } 
            else                                                                            // this parameter is crucial => throw exception 
            { 
                RCLCPP_FATAL(this->get_logger(), "...... Boundary 2 out of expected range (%d [0.01°])!", rod_settings.scan_angle_max);
                throw std::runtime_error("Scanning Angle Range (Boundary 2) is out of expected range!");
            } 
            if(rod_settings.scan_angle_min < rod_settings.scan_angle_max)
            {
                // OK
            }
            else                                                                            // this parameter is crucial => throw exception 
            {
                RCLCPP_FATAL(this->get_logger(), "...... Boundary 1 (%d [0.01°]) must be smaller then Boundary 2 (%d [0.01°])!", rod_settings.scan_angle_min, rod_settings.scan_angle_max);
                throw std::runtime_error("Scanning Angle Ranges are not set properly");
            }  
                            
            /* Scanning frequency / Angular resolution */
            switch(rod_settings.scan_resolution)
            {
                case RODPAR_RESOL_0200_80_Hz: 
                    RCLCPP_INFO(this->get_logger(), "... Angular resolution / Scanning Frequency = 0.2° @ 80 Hz");
                    laser_scan_params_.angle_resolution = 0.2f;
                    laser_scan_params_.scan_duration    = 1.0f / 80.0f;
                    break;

                case RODPAR_RESOL_0200_50_Hz: 
                    RCLCPP_INFO(this->get_logger(), "... Angular resolution / Scanning Frequency = 0.2° @ 50 Hz");
                    laser_scan_params_.angle_resolution = 0.2f;
                    laser_scan_params_.scan_duration    = 1.0f / 50.0f;
                    break;

                case RODPAR_RESOL_0100_40_Hz:
                    RCLCPP_INFO(this->get_logger(), "... Angular resolution / Scanning Frequency = 0.1° @ 40 Hz");
                    laser_scan_params_.angle_resolution = 0.1f;
                    laser_scan_params_.scan_duration    = 1.0f / 40.0f;
                    break;

                case RODPAR_RESOL_0050_20_Hz:
                    RCLCPP_INFO(this->get_logger(), "... Angular resolution / Scanning Frequency = 0.05° @ 20 Hz");
                    laser_scan_params_.angle_resolution = 0.05f;
                    laser_scan_params_.scan_duration    = 1.0f / 20.0f;
                    break;

                case RODPAR_RESOL_0025_10_Hz:
                    RCLCPP_INFO(this->get_logger(), "... Angular resolution / Scanning Frequency = 0.025° @ 10 Hz");
                    laser_scan_params_.angle_resolution = 0.025f;
                    laser_scan_params_.scan_duration    = 1.0f / 10.0f;
                    break;

                default:                                                                    // this parameter is crucial => throw exception 
                    RCLCPP_FATAL(this->get_logger(), "... Angular resolution / Scanning Frequency out of expected range (%d)!", rod_settings.scan_resolution);
                    throw std::runtime_error("Angular resolution / Scanning requency is out of expected range!");
                    break;
            }

            /* Skip spot */
            laser_scan_params_.skip_spots = rod_settings.scan_skip_spots;
            if(laser_scan_params_.skip_spots <= RODRNG_SKIPSPOT_MAX)
            { 
                RCLCPP_INFO(this->get_logger(), "... Skip Spot = %d", laser_scan_params_.skip_spots);
            }   
            else                                                                            // this parameter is crucial => throw exception 
            { 
                RCLCPP_FATAL(this->get_logger(), "... Skip Spot out of expected range (%d)!", laser_scan_params_.skip_spots);
                throw std::runtime_error("Skip Spot is out of expected range!");
            } 

            /* Filter type */
            switch(rod_settings.filter_type)
            {
                case RODPAR_FILTER_MEDIAN:
                    RCLCPP_INFO(this->get_logger(), "... Filter type = Median");
                    break;

                case RODPAR_FILTER_AVERAGE:
                    RCLCPP_INFO(this->get_logger(), "... Filter type = Average");
                    break;

                case RODPAR_FILTER_MAX:
                    RCLCPP_INFO(this->get_logger(), "... Filter type = Max");
                    break;

                case RODPAR_FILTER_COMBO:
                    RCLCPP_INFO(this->get_logger(), "... Filter type = Combo");
                    break;

                default:
                    RCLCPP_WARN(this->get_logger(), "... Filter type out of expected range (%d)!", rod_settings.filter_type);
                    break;
            } 
            if(rod_settings.filter_spots_nghb <= RODRNG_FILTER_SPOTSNGBH_MAX)
            { 
                RCLCPP_INFO(this->get_logger(), "...... Neighboring Spots = %d", rod_settings.filter_spots_nghb);
            } 
            else
            { 
                RCLCPP_WARN(this->get_logger(), "... Neighboring Spots out of expected range (%d)!", rod_settings.filter_spots_nghb);
            } 
            if(rod_settings.filter_spots_hist <= RODRNG_FILTER_SPOTSHIST_MAX)
            { 
                RCLCPP_INFO(this->get_logger(), "...... Historical Spots = %d", rod_settings.filter_spots_hist);
            } 
            else
            { 
                RCLCPP_WARN(this->get_logger(), "... Historical Spots out of expected range (%d)!", rod_settings.filter_spots_hist);
            }            

            /* Contamination threshold */
            RCLCPP_INFO(this->get_logger(), "... Contamination threshold");
            if(rod_settings.cont_threshold_warn <= RODRNG_CONT_MAX)
            { 
                RCLCPP_INFO(this->get_logger(), "...... Warning = %d %%", rod_settings.cont_threshold_warn);
            } 
            else
            { 
                RCLCPP_WARN(this->get_logger(), "... Warning out of expected range (%d)!", rod_settings.cont_threshold_warn);
            } 
            if(rod_settings.cont_threshold_error <= RODRNG_CONT_MAX)
            { 
                RCLCPP_INFO(this->get_logger(), "...... Error = %d %%", rod_settings.cont_threshold_error);
            } 
            else
            { 
                RCLCPP_WARN(this->get_logger(), "... Error out of expected range (%d)!", rod_settings.cont_threshold_error);
            } 
            if(rod_settings.cont_threshold_warn < rod_settings.cont_threshold_error)
            {
                // OK
            } 
            else
            {
                RCLCPP_WARN(this->get_logger(), "...... Warning threshold (%d %%) is greater than Error threshold (%d %%)!", rod_settings.cont_threshold_warn, rod_settings.cont_threshold_error);
            } 
            
            /* Data packet type */
            laser_scan_params_.packet_type = rod_settings.mdi_packet_type;
            switch(laser_scan_params_.packet_type)
            {
                case RODPAR_PTYPE_DISTANCES_ONLY:
                    RCLCPP_INFO(this->get_logger(), "... Data packet type = Distance");
                    break;

                case RODPAR_PTYPE_DISTANCE_AND_AMPLITUDE:
                    RCLCPP_INFO(this->get_logger(), "... Data packet type = Distance & Amplitude");
                    break;

                default:                                                                    // this parameter is crucial => throw exception 
                    RCLCPP_FATAL(this->get_logger(), "... Data packet type out of expected range (%d)!", laser_scan_params_.packet_type);
                    throw std::runtime_error("Unknown Data packet type!");
                    break;
            } 

            /* Data Output Direction */
            laser_scan_params_.scan_direction = rod_settings.mdi_data_direction;
            switch(laser_scan_params_.scan_direction)
            {    
                case RODPAR_DIR_CLOCKWISE:
                    RCLCPP_INFO(this->get_logger(), "... Data output direction = Clockwise");
                    laser_scan_params_.angle_min    = float(-rod_settings.scan_angle_min) / 100.0f * CONST_1_DEGREE_IN_RADIANS;
                    laser_scan_params_.angle_max    = float(-rod_settings.scan_angle_max) / 100.0f * CONST_1_DEGREE_IN_RADIANS;
                    laser_scan_params_.angle_delta  = -laser_scan_params_.angle_resolution * float(laser_scan_params_.skip_spots + 1) * CONST_1_DEGREE_IN_RADIANS;
                    break;

                case RODPAR_DIR_COUNTERCLOCKWISE:
                    RCLCPP_INFO(this->get_logger(), "... Data output direction = Counterclockwise");
                    laser_scan_params_.angle_min    = float(-rod_settings.scan_angle_max) / 100.0f * CONST_1_DEGREE_IN_RADIANS;
                    laser_scan_params_.angle_max    = float(-rod_settings.scan_angle_min) / 100.0f * CONST_1_DEGREE_IN_RADIANS;
                    laser_scan_params_.angle_delta  = laser_scan_params_.angle_resolution * float(laser_scan_params_.skip_spots + 1) * CONST_1_DEGREE_IN_RADIANS;
                    break;

                default:                                                                    // this parameter is crucial => throw exception 
                    RCLCPP_FATAL(this->get_logger(), "... Data output direction out of expected range (%d)!", laser_scan_params_.scan_direction);
                    throw std::runtime_error("Unknown Data output direction!");
                    break;
            } 

            /* Data Output Protocol */
            laser_scan_params_.protocol = rod_settings.mdi_protocol;
            switch(laser_scan_params_.protocol)
            {
                case RODPAR_PROTO_UDP:
                    RCLCPP_INFO(this->get_logger(), "... Data Output Protocol = UDP");
                    RCLCPP_INFO(this->get_logger(), "------------------------------------------------------------");
                    if(eth_comm_ifc_->startCapturingUDP())                                  // initialite UDP scan data receiver
                    {  
                        RCLCPP_INFO(this->get_logger(), "Capturing UDP data...");
                    } 
                    else                                                                    // this parameter is crucial => throw exception 
                    { 
                        RCLCPP_FATAL(this->get_logger(), "Capturing of UDP data failed");
                        throw std::runtime_error("Capturing of UDP data failed!");
                    } 
                    break;

                case RODPAR_PROTO_TCP:
                    RCLCPP_INFO(this->get_logger(), "... Data Output Protocol = TCP");
                    RCLCPP_INFO(this->get_logger(), "------------------------------------------------------------");
                    if(eth_comm_ifc_->startCapturingTCP())                                  // initialite TCP scan data receiver
                    { 
                        RCLCPP_INFO(this->get_logger(), "Capturing TCP data...");
                    } 
                    else                                                                    // this parameter is crucial => throw exception 
                    { 
                        RCLCPP_FATAL(this->get_logger(), "Capturing of TCP data failed");
                        throw std::runtime_error("Capturing of TCP data failed!");
                    } 
                    break;

                default:                                                                    // this parameter is crucial => throw exception 
                    RCLCPP_FATAL(this->get_logger(), "... Data output Protocol out of expected range (%d)!", laser_scan_params_.protocol);
                    throw std::runtime_error("Unknown Data output Protocol!");
                    break;
            }
        }
    } 

    /****************************************************************************************************/

    /**
     * @brief print header and info about readed parameters
     */
    void RodNode::logInfoDriverStartup()
    {
        /* print basic info about node */
        RCLCPP_INFO(this->get_logger(), "============================================================");
        RCLCPP_INFO(this->get_logger(), "Leuze RODxxx driver");
        RCLCPP_INFO(this->get_logger(), "============================================================");
        
        /* log loaded parameter values */
        RCLCPP_INFO(this->get_logger(), "Loaded parameter values...");
        RCLCPP_INFO(this->get_logger(), "... ip_addr  = \"%s\"", ip_addr_.c_str());
        RCLCPP_INFO(this->get_logger(), "... port_num = \"%d\"", port_num_);
        RCLCPP_INFO(this->get_logger(), "... frame_id = \"%s\"", frame_id_.c_str());
        RCLCPP_INFO(this->get_logger(), "... topic_id = \"%s\"", topic_id_.c_str());
        RCLCPP_INFO(this->get_logger(), "------------------------------------------------------------");
    } 

    /**
     * @brief print info about used frames and topics
     */
    void RodNode::logInfoTopicsAndFrame()
    {
        RCLCPP_INFO(this->get_logger(), "Publishing data...");
        RCLCPP_INFO(this->get_logger(), "... Scan data frame   = \"%s\"", frame_id_.c_str());
        RCLCPP_INFO(this->get_logger(), "... Scan data topic   = \"%s\"", topic_id_.c_str());
        #ifdef STATUS 
            RCLCPP_INFO(this->get_logger(), "... Status data topic = \"%s_status\"", topic_id_.c_str());
        #else 
            RCLCPP_INFO(this->get_logger(), "------------------------------------------------------------");
            RCLCPP_WARN(this->get_logger(), "!!! STATUS data publishing is disabled !!!");
            RCLCPP_WARN(this->get_logger(), "No status data of a ROD x08 laser scanner will be");
            RCLCPP_WARN(this->get_logger(), "communicated and published. If needed, check CMakeLists.txt");
            RCLCPP_WARN(this->get_logger(), "and uncomment add_definitions(-DSTATUS) to enable it.");
        #endif
        RCLCPP_INFO(this->get_logger(), "============================================================");
    } 

    /**
     * @brief print warning message about that debug mode has been enabled
     */
    void RodNode::logWarnDebug()
    {
        RCLCPP_WARN(this->get_logger(), "!!! DEBUG mode is enabled !!!");
        RCLCPP_WARN(this->get_logger(), "It is recommended to run the ROD driver withouth enabled");
        RCLCPP_WARN(this->get_logger(), "debug mode. Please, check CMakeLists.txt and comment out");
        RCLCPP_WARN(this->get_logger(), "add_definitions(-DDEBUG) to disable it.");
        RCLCPP_INFO(this->get_logger(), "------------------------------------------------------------");
    } 

    /**
     * @brief print warning message about that simulation mode has been enabled
     */
    void RodNode::logWarnSimulation()
    {
        RCLCPP_WARN(this->get_logger(), "!!! SIMULATION mode is enabled !!!");
        RCLCPP_WARN(this->get_logger(), "No connection with a real ROD x08 laser scanner will be");
        RCLCPP_WARN(this->get_logger(), "estabilished and only a simulation data will be published.");
        RCLCPP_WARN(this->get_logger(), "Please, check CMakeLists.txt and comment out");
        RCLCPP_WARN(this->get_logger(), "add_definitions(-DSIMULATION) to disable it.");
        RCLCPP_INFO(this->get_logger(), "------------------------------------------------------------");
    } 
};
