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

#include <iostream>

#include <rclcpp/rclcpp.hpp>
#include <rclcpp/time.hpp>

#include <boost/asio.hpp>

#include "leuze_rod_driver/eth_comm_tcp.h"
#include "leuze_rod_driver/common.h"

namespace leuze_rod_ros2_drivers
{
    /************************************************************************************************************
     * @class   EthCommTCP
     * 
     * @brief   This class ensures TCP commmunication with a ROD scanner. It supports capturing of a TCP process
     *          data (MDI packets) and communication of settings and status (command interface)
     ************************************************************************************************************/
    
    /**
     *@brief    constructor
     *@param    hostname
     *@param    tcp_port
     */
    EthCommTCP::EthCommTCP(const std::string hostname, const int tcp_port) : ScanDataParser(), inbuf_(4096), instream_(&inbuf_)
    {
        tcp_socket_         = nullptr;

        pending_req_        = CMDITF_REQ_NULL;
        pending_resp_       = CMDITF_RESP_NULL;

        state_connected_    = false;
        state_capturing_    = false;
        
        try
        {
            // resolve hostname/ip
            boost::asio::ip::tcp::resolver resolver(io_service_);
            boost::asio::ip::tcp::resolver::query query(hostname, std::to_string(tcp_port));
            boost::asio::ip::tcp::resolver::iterator endpoint_iterator = resolver.resolve(query);
            boost::asio::ip::tcp::resolver::iterator end;

            tcp_socket_ = new boost::asio::ip::tcp::socket(io_service_);
            boost::system::error_code error = boost::asio::error::host_not_found;

            // iterate over endpoints and etablish connection
            while(error && endpoint_iterator != end)
            {
                tcp_socket_->close();
                tcp_socket_->connect(*endpoint_iterator++, error);
            }
            if(error)
            {
                throw boost::system::system_error(error);
            }

            // start async reading
            tcp_socket_->async_receive(boost::asio::buffer(&tcp_buffer_[0],tcp_buffer_.size()),\
                                        boost::bind(&EthCommTCP::tcpSockRecvHandler, this,\
                                        boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred));
            io_service_thread_ = boost::thread(boost::bind(&boost::asio::io_service::run, &io_service_));

            // initialize mutexes
            mutex_tcp_write_.unlock();
            mutex_rod_status_.unlock();
            mutex_send_mdi_.lock();  
            mutex_stop_mdi_.lock();  
            mutex_get_proto_.lock();    
            mutex_get_ptype_.lock();   
            mutex_get_resol_.lock();    
            mutex_get_dir_.lock();    
            mutex_get_range_.lock();    
            mutex_get_skip_.lock();    
            mutex_get_cont_.lock();    
            mutex_get_winstat_.lock();    
            mutex_get_ver_.lock();    
            mutex_get_tem_.lock();   
            mutex_get_elog_.lock();  
            mutex_get_hours_.lock();    
            mutex_get_wcalib_.lock();    
            mutex_get_filter_.lock();    
            mutex_get_ecode_.lock();    
            mutex_get_txmdi_.lock();  

            state_connected_ = true;
        }
        catch (std::exception& e)
        {
            DEBUG_ERR("[EthCommTCP] Exception: " <<  e.what());
        }
    }

    /**
     *@brief destructor 
     */
    EthCommTCP::~EthCommTCP()
    {
        disconnect();

        delete tcp_socket_;
        tcp_socket_ = nullptr;
    }

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

    /**
     *@brief    handle opened TCP socket receiving and data parsing
     *@param    error
     *@param    bytes_transferred 
     */
    void EthCommTCP::tcpSockRecvHandler(const boost::system::error_code& error, std::size_t bytes_transferred)
    {
        std::string str_tcp_buffer(tcp_buffer_.data(), bytes_transferred);

        /* find a command interface response in the TCP buffer */
        std::size_t pos_cmditf_resp_start   = 0;
        std::size_t pos_cmditf_resp_end     = 0;

        for(std::size_t a = 0; a < bytes_transferred - 5; a++)                                                                      // response has format: <STX>cRA{SPC}...<ETX> or <STX>cWA{SPC}...<ETX>  
        {
            if(tcp_buffer_[a] == 0x02)                                                                                              // char <STX> is 0x02 
            {
                if(tcp_buffer_[a + 3] == 'A')
                {
                    if(tcp_buffer_[a + 1] == 'c' && (tcp_buffer_[a + 2] == 'R' || tcp_buffer_[a + 2] == 'W') && tcp_buffer_[a + 4] == ' ')
                    {
                        pos_cmditf_resp_start = a;
                        for(std::size_t b = a + 5; b < bytes_transferred; b++)
                        {
                            if(tcp_buffer_[b] == 0x03)                                                                              // char <ETX> is 0x03
                            {
                                pos_cmditf_resp_end = b;
                                break;
                            } 
                            else continue;
                        }
                        break; 
                    } 
                    else continue;  
                }
                else continue;
            }
            else continue;
        }

        /* parce Command interface response (if any) and MDI data */
        if(pos_cmditf_resp_end > 0 && pos_cmditf_resp_start < pos_cmditf_resp_end)
        {
            std::string str_cmditf_resp(tcp_buffer_.data() + pos_cmditf_resp_start, pos_cmditf_resp_end - pos_cmditf_resp_start);   // separate command interface response from TCP buffer string
            
            DEBUG_LOG("[tcpSockRecvHandler] " << str_cmditf_resp);
            parseCmdItfResp(str_cmditf_resp);

            if(state_capturing_)
            {
                str_tcp_buffer.erase(pos_cmditf_resp_start, str_cmditf_resp.length());                                                  // remove command interface response from TCP buffer string
                writeToRingBuff(str_tcp_buffer.c_str(), str_tcp_buffer.length());
            }
            else
            {
            }  
        } 
        else
        {
            if(state_capturing_)
            {
                writeToRingBuff(&tcp_buffer_[0], bytes_transferred);
            }
            else
            {
            } 
        } 

        parsePacketsFromRingBuff();

        /* Error handling and data receiving */
        if(error)
        {
            DEBUG_ERR("[tcpSockRecvHandler] Error: " << error.message() << "(" << error.value() << ")");
            disconnect();
        }
        else
        {           
            // Read all received data and write it to the internal ring buffer
            instream_.clear();
            while(!instream_.eof())
            {
                char buf[SCANPAR_BUFF_SIZE_IN];
                instream_.read(buf, SCANPAR_BUFF_SIZE_IN);
            }

            // Read data asynchronously
            tcp_socket_->async_receive(boost::asio::buffer(&tcp_buffer_[0], tcp_buffer_.size()), boost::bind(&EthCommTCP::tcpSockRecvHandler, this, boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred));
        }
    }

    /**
     *@brief    handle opened TCP socket write 
     */
    boost::system::error_code EthCommTCP::tcpSockSendCommand(const unsigned int cmd)
    {
        boost::system::error_code ec;

        if(mutex_tcp_write_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
        {
            switch (cmd)
            {
                case CMDITF_REQ_SEND_MDI:
                    pending_req_ = CMDITF_REQ_SEND_MDI;
                    tcp_socket_->write_some(boost::asio::buffer(CMDITF_CWN_SEND_MDI, sizeof(CMDITF_CWN_SEND_MDI)), ec); 
                    DEBUG_LOG("[tcpSockSendCommand] " << CMDITF_CWN_SEND_MDI);
                    break;
                    
                case CMDITF_REQ_STOP_MDI:
                    pending_req_ = CMDITF_REQ_STOP_MDI;
                    tcp_socket_->write_some(boost::asio::buffer(CMDITF_CWN_STOP_MDI, sizeof(CMDITF_CWN_STOP_MDI)), ec); 
                    DEBUG_LOG("[tcpSockSendCommand] " << CMDITF_CWN_STOP_MDI);
                    break;

                case CMDITF_REQ_GET_PROTO:
                    pending_req_ = CMDITF_REQ_GET_PROTO;
                    tcp_socket_->write_some(boost::asio::buffer(CMDITF_CRN_GET_PROTO, sizeof(CMDITF_CRN_GET_PROTO)), ec); 
                    DEBUG_LOG("[tcpSockSendCommand] " << CMDITF_CRN_GET_PROTO);
                    break;

                case CMDITF_REQ_GET_PTYPE:
                    pending_req_ = CMDITF_REQ_GET_PTYPE;
                    tcp_socket_->write_some(boost::asio::buffer(CMDITF_CRN_GET_PTYPE, sizeof(CMDITF_CRN_GET_PTYPE)), ec); 
                    DEBUG_LOG("[tcpSockSendCommand] " << CMDITF_CRN_GET_PTYPE);
                    break;

                case CMDITF_REQ_GET_RESOL:
                    pending_req_ = CMDITF_REQ_GET_RESOL;
                    tcp_socket_->write_some(boost::asio::buffer(CMDITF_CRN_GET_RESOL, sizeof(CMDITF_CRN_GET_RESOL)), ec); 
                    DEBUG_LOG("[tcpSockSendCommand] " << CMDITF_CRN_GET_RESOL);
                    break;

                case CMDITF_REQ_GET_DIR:
                    pending_req_ = CMDITF_REQ_GET_DIR;
                    tcp_socket_->write_some(boost::asio::buffer(CMDITF_CRN_GET_DIR, sizeof(CMDITF_CRN_GET_DIR)), ec); 
                    DEBUG_LOG("[tcpSockSendCommand] " << CMDITF_CRN_GET_DIR);
                    break;

                case CMDITF_REQ_GET_RANGE:
                    pending_req_ = CMDITF_REQ_GET_RANGE;
                    tcp_socket_->write_some(boost::asio::buffer(CMDITF_CRN_GET_RANGE, sizeof(CMDITF_CRN_GET_RANGE)), ec); 
                    DEBUG_LOG("[tcpSockSendCommand] " << CMDITF_CRN_GET_RANGE);
                    break;

                case CMDITF_REQ_GET_SKIP:
                    pending_req_ = CMDITF_REQ_GET_SKIP;
                    tcp_socket_->write_some(boost::asio::buffer(CMDITF_CRN_GET_SKIP, sizeof(CMDITF_CRN_GET_SKIP)), ec); 
                    DEBUG_LOG("[tcpSockSendCommand] " << CMDITF_CRN_GET_SKIP);
                    break;

                case CMDITF_REQ_GET_CONT:
                    pending_req_ = CMDITF_REQ_GET_CONT;
                    tcp_socket_->write_some(boost::asio::buffer(CMDITF_CRN_GET_CONT, sizeof(CMDITF_CRN_GET_CONT)), ec); 
                    DEBUG_LOG("[tcpSockSendCommand] " << CMDITF_CRN_GET_CONT);
                    break;

                case CMDITF_REQ_GET_WINSTAT:   
                    pending_req_ = CMDITF_REQ_GET_WINSTAT;
                    tcp_socket_->write_some(boost::asio::buffer(CMDITF_CRN_GET_WINSTAT, sizeof(CMDITF_CRN_GET_WINSTAT)), ec); 
                    DEBUG_LOG("[tcpSockSendCommand] " << CMDITF_CRN_GET_WINSTAT);
                    break;

                case CMDITF_REQ_GET_VER:
                    pending_req_ = CMDITF_REQ_GET_VER;
                    tcp_socket_->write_some(boost::asio::buffer(CMDITF_CRN_GET_VER, sizeof(CMDITF_CRN_GET_VER)), ec); 
                    DEBUG_LOG("[tcpSockSendCommand] " << CMDITF_CRN_GET_VER);
                    break;

                case CMDITF_REQ_GET_TEM:
                    pending_req_ = CMDITF_REQ_GET_TEM;
                    tcp_socket_->write_some(boost::asio::buffer(CMDITF_CRN_GET_TEM, sizeof(CMDITF_CRN_GET_TEM)), ec); 
                    DEBUG_LOG("[tcpSockSendCommand] " << CMDITF_CRN_GET_TEM);
                    break;

                case CMDITF_REQ_GET_ELOG:
                    pending_req_ = CMDITF_REQ_GET_ELOG;
                    tcp_socket_->write_some(boost::asio::buffer(CMDITF_CRN_GET_ELOG, sizeof(CMDITF_CRN_GET_ELOG)), ec); 
                    DEBUG_LOG("[tcpSockSendCommand] " << CMDITF_CRN_GET_ELOG);
                    break;

                case CMDITF_REQ_GET_HOURS:
                    pending_req_ = CMDITF_REQ_GET_HOURS;
                    tcp_socket_->write_some(boost::asio::buffer(CMDITF_CRN_GET_HOURS, sizeof(CMDITF_CRN_GET_HOURS)), ec); 
                    DEBUG_LOG("[tcpSockSendCommand] " << CMDITF_CRN_GET_HOURS);
                    break;

                case CMDITF_REQ_GET_WCALIB:
                    pending_req_ = CMDITF_REQ_GET_WCALIB;
                    tcp_socket_->write_some(boost::asio::buffer(CMDITF_CRN_GET_WCALIB, sizeof(CMDITF_CRN_GET_WCALIB)), ec); 
                    DEBUG_LOG("[tcpSockSendCommand] " << CMDITF_CRN_GET_WCALIB);
                    break;

                case CMDITF_REQ_GET_FILTER:
                    pending_req_ = CMDITF_REQ_GET_FILTER;
                    tcp_socket_->write_some(boost::asio::buffer(CMDITF_CRN_GET_FILTER, sizeof(CMDITF_CRN_GET_FILTER)), ec); 
                    DEBUG_LOG("[tcpSockSendCommand] " << CMDITF_CRN_GET_FILTER);
                    break;

                case CMDITF_REQ_GET_ECODE:
                    pending_req_ = CMDITF_REQ_GET_ECODE;
                    tcp_socket_->write_some(boost::asio::buffer(CMDITF_CRN_GET_ECODE, sizeof(CMDITF_CRN_GET_ECODE)), ec);
                    DEBUG_LOG("[tcpSockSendCommand] " << CMDITF_CRN_GET_ECODE);
                    break;

                case CMDITF_REQ_GET_TXMDI:
                    pending_req_ = CMDITF_REQ_GET_TXMDI;
                    tcp_socket_->write_some(boost::asio::buffer(CMDITF_CRN_GET_TXMDI, sizeof(CMDITF_CRN_GET_TXMDI)), ec);
                    DEBUG_LOG("[tcpSockSendCommand] " << CMDITF_CRN_GET_TXMDI);
                    break;

                default:
                    pending_req_ = CMDITF_REQ_NULL;
                    DEBUG_ERR("[tcpSockRecvHandler] Unknown request ID!");
                    break;
            }
            mutex_tcp_write_.unlock();
        } 
        else
        {
            DEBUG_ERR("[tcpSockSendCommand] Timeout!!!");
        } 

        return ec;
    }

    bool EthCommTCP::parseCmdItfResp(std::string response_string)
    {
        bool retval = true;

        std::string::size_type position;

        switch(pending_req_)
        {
            /* START MDI */
            case CMDITF_REQ_SEND_MDI:
                if(response_string.compare(CMDITF_CWA_SEND_MDI))
                {
                    pending_resp_ = CMDITF_RESP_SEND_MDI;
                }
                else
                {
                    DEBUG_ERR("[tcpSockRecvHandler] Unable to start sending of MDI");
                }
                mutex_send_mdi_.unlock();  
                break;

            /* STOP MDI */
            case CMDITF_REQ_STOP_MDI:
                if(response_string.compare(CMDITF_CWA_STOP_MDI))
                {
                    pending_resp_ = CMDITF_RESP_STOP_MDI;
                }
                else
                {
                    DEBUG_ERR("[tcpSockRecvHandler] Unable to stop sending of MDI");
                }
                mutex_stop_mdi_.unlock(); 
                break;

            /* GET RESOLUTION */
            case CMDITF_REQ_GET_RESOL:
                position = response_string.find(CMDITF_CRA_GET_RESOL);
                if(position != response_string.npos)
                {
                    std::vector<std::string> params = splitString(response_string, ' ');
                    if(params.size() == 3)
                    {
                        pending_resp_ = CMDITF_RESP_GET_RESOL;
                        mutex_rod_status_.lock();
                        rod_status_.scan_resolution = std::stoi(params[2]);
                        mutex_rod_status_.unlock();
                    }
                    else
                    {
                        DEBUG_ERR("[tcpSockRecvHandler] Number of parameters does not match");
                    }
                }
                else
                {
                    DEBUG_ERR("[tcpSockRecvHandler] Unable to parse parameters from response_string");
                }
                mutex_get_resol_.unlock(); 
                break;

            /* GET ANGLE RANGE */
            case CMDITF_REQ_GET_RANGE:
                position = response_string.find(CMDITF_CRA_GET_RANGE);
                if (position != response_string.npos)
                {
                    std::vector<std::string> params = splitString(response_string,' ');
                    if(params.size() == 4)
                    {
                        pending_resp_ = CMDITF_RESP_GET_RANGE;
                        mutex_rod_status_.lock();
                        rod_status_.scan_angle_min = std::stoi(params[2]);
                        rod_status_.scan_angle_max = std::stoi(params[3]);
                        mutex_rod_status_.unlock();
                    }
                    else
                    {
                        DEBUG_ERR("[tcpSockRecvHandler] Number of parameters does not match");
                    }
                }
                else
                {
                    DEBUG_ERR("[tcpSockRecvHandler] Unable to parse parameters from response_string");
                }
                mutex_get_range_.unlock(); 
                break;

            /* GET MDI PACKET TYPE */
            case CMDITF_REQ_GET_PTYPE:
                position = response_string.find(CMDITF_CRA_GET_PTYPE);
                if (position != response_string.npos)
                {
                    std::vector<std::string> params = splitString(response_string,' ');
                    if(params.size() == 3)
                    {
                        pending_resp_ = CMDITF_RESP_GET_PTYPE;
                        mutex_rod_status_.lock();
                        rod_status_.mdi_packet_type = std::stoi(params[2]);
                        mutex_rod_status_.unlock();
                    }
                    else
                    {
                        DEBUG_ERR("[tcpSockRecvHandler] Number of parameters does not match");
                    }
                }
                else
                {
                    DEBUG_ERR("[tcpSockRecvHandler] Unable to parse parameters from response_string");
                }
                mutex_get_ptype_.unlock();    
                break;

            /* GET MDI PROTOCOL TYPE */
            case CMDITF_REQ_GET_PROTO:
                position = response_string.find(CMDITF_CRA_GET_PROTO);
                if (position != response_string.npos)
                {
                    std::vector<std::string> params = splitString(response_string,' ');
                    if(params.size() == 3)
                    {
                        pending_resp_ = CMDITF_RESP_GET_PROTO;
                        mutex_rod_status_.lock();
                        rod_status_.mdi_protocol = std::stoi(params[2]);
                        mutex_rod_status_.unlock();
                    }
                    else
                    {
                        DEBUG_ERR("[tcpSockRecvHandler] Number of parameters does not match");
                    }
                }
                else
                {
                    DEBUG_ERR("[tcpSockRecvHandler] Unable to parse parameters from response_string");
                }
                mutex_get_proto_.unlock();
                break;

            /* GET MDI DATA DIRECTION */
            case CMDITF_REQ_GET_DIR:
                position = response_string.find(CMDITF_CRA_GET_DIR);
                if (position != response_string.npos)
                {
                    std::vector<std::string> params = splitString(response_string,' ');
                    if(params.size() == 3)
                    {
                        pending_resp_ = CMDITF_RESP_GET_DIR;
                        mutex_rod_status_.lock();
                        rod_status_.mdi_data_direction = std::stoi(params[2]);
                        mutex_rod_status_.unlock();
                    }
                    else
                    {
                        DEBUG_ERR("[tcpSockRecvHandler] Number of parameters does not match");
                    }
                }
                else
                {
                    DEBUG_ERR("[tcpSockRecvHandler] Unable to parse parameters from response_string");
                }
                mutex_get_dir_.unlock();                                                    
                break;

            /* GET SCAN SKIP POINTS */
            case CMDITF_REQ_GET_SKIP:
                position = response_string.find(CMDITF_CRA_GET_SKIP);
                if (position != response_string.npos)
                {
                    std::vector<std::string> params = splitString(response_string,' ');
                    if(params.size() == 3)
                    {
                        pending_resp_ = CMDITF_RESP_GET_SKIP;
                        mutex_rod_status_.lock();
                        rod_status_.scan_skip_spots = std::stoi(params[2]);
                        mutex_rod_status_.unlock();
                    }
                    else
                    {
                        DEBUG_ERR("[tcpSockRecvHandler] Number of parameters does not match");
                    }
                }
                else
                {
                    DEBUG_ERR("[tcpSockRecvHandler] Unable to parse parameters from response_string");
                }
                mutex_get_skip_.unlock();   
                break;

            /* GET CONTAMINATION THRESHOLD*/
            case CMDITF_REQ_GET_CONT:
                position = response_string.find(CMDITF_CRA_GET_CONT);
                if (position != response_string.npos)
                {
                    std::vector<std::string> params = splitString(response_string,' ');
                    if(params.size() == 4)
                    {
                        pending_resp_ = CMDITF_RESP_GET_CONT;
                        mutex_rod_status_.lock();
                        rod_status_.cont_threshold_warn     = std::stoi(params[2]);
                        rod_status_.cont_threshold_error    = std::stoi(params[3]);
                        mutex_rod_status_.unlock();
                    }
                    else
                    {
                        DEBUG_ERR("[tcpSockRecvHandler] Number of parameters does not match");
                    }
                }
                else
                {
                    DEBUG_ERR("[tcpSockRecvHandler] Unable to parse parameters from response_string");
                }
                mutex_get_cont_.unlock();
                break;

            /* GET CONTAMINATION STATE */
            case CMDITF_REQ_GET_WINSTAT:   
                position = response_string.find(CMDITF_CRA_GET_WINSTAT);
                if (position != response_string.npos)
                {
                    std::vector<std::string> params = splitString(response_string,' ');
                    if(params.size() == 11)
                    {
                        pending_resp_ = CMDITF_RESP_GET_WINSTAT;
                        mutex_rod_status_.lock();
                        for(int i = 0; i < 9; i++)
                        {
                            rod_status_.cont_state[i]    = std::stoi(params[2 + i]);
                        } 
                        mutex_rod_status_.unlock();
                    }
                    else
                    {
                        DEBUG_ERR("[tcpSockRecvHandler] Number of parameters does not match");
                    }
                }
                else
                {
                    DEBUG_ERR("[tcpSockRecvHandler] Unable to parse parameters from response_string");
                }
                mutex_get_winstat_.unlock();
                break;

            /* GET VERSION INFO */
            case CMDITF_REQ_GET_VER:
                position = response_string.find(CMDITF_CRA_GET_VER);
                if (position != response_string.npos)
                {
                    std::vector<std::string> params = splitString(response_string,' ');
                    if(params.size() == 9)
                    {
                        pending_resp_ = CMDITF_RESP_GET_VER;
                        mutex_rod_status_.lock();
                        rod_status_.rod_part_number = std::stoi(params[2]);
                        rod_status_.rod_hw_ver      = std::stoi(params[3]);
                        rod_status_.rod_sw_ver      = std::stoi(params[4]);
                        mutex_rod_status_.unlock();
                    }
                    else
                    {
                        DEBUG_ERR("[tcpSockRecvHandler] Number of parameters does not match");
                    }
                }
                else
                {
                    DEBUG_ERR("[tcpSockRecvHandler] Unable to parse parameters from response_string");
                }
                mutex_get_ver_.unlock();
                break;

            /* GET TEMPERATURE */
            case CMDITF_REQ_GET_TEM:
                position = response_string.find(CMDITF_CRA_GET_TEM);
                if (position != response_string.npos)
                {
                    std::vector<std::string> params = splitString(response_string,' ');
                    if(params.size() == 3)
                    {
                        pending_resp_ = CMDITF_RESP_GET_TEM;
                        mutex_rod_status_.lock();
                        rod_status_.rod_temp = std::stoi(params[2]);
                        mutex_rod_status_.unlock();
                    }
                    else
                    {
                        DEBUG_ERR("[tcpSockRecvHandler] Number of parameters does not match");
                    }
                }
                else
                {
                    DEBUG_ERR("[tcpSockRecvHandler] Unable to parse parameters from response_string");
                }
                mutex_get_tem_.unlock();
                break;

            /* GET ERROR LOG */
            case CMDITF_REQ_GET_ELOG:
                position = response_string.find(CMDITF_CRA_GET_ELOG);
                if (position != response_string.npos)
                {
                    std::vector<std::string> params = splitString(response_string,' ');
                    if(params.size() == 23)
                    {
                        pending_resp_ = CMDITF_RESP_GET_ELOG;
                        mutex_rod_status_.lock();
                        rod_status_.error_log_code[0] = std::stoi(params[3]);
                        rod_status_.error_log_date[0] = std::stoi(params[4]);
                        rod_status_.error_log_code[1] = std::stoi(params[5]);
                        rod_status_.error_log_date[1] = std::stoi(params[6]);
                        rod_status_.error_log_code[2] = std::stoi(params[7]);
                        rod_status_.error_log_date[2] = std::stoi(params[8]);
                        rod_status_.error_log_code[3] = std::stoi(params[9]);
                        rod_status_.error_log_date[3] = std::stoi(params[10]);
                        rod_status_.error_log_code[4] = std::stoi(params[11]);
                        rod_status_.error_log_date[4] = std::stoi(params[12]);
                        rod_status_.error_log_code[5] = std::stoi(params[13]);
                        rod_status_.error_log_date[5] = std::stoi(params[14]);
                        rod_status_.error_log_code[6] = std::stoi(params[15]);
                        rod_status_.error_log_date[6] = std::stoi(params[16]);
                        rod_status_.error_log_code[7] = std::stoi(params[17]);
                        rod_status_.error_log_date[7] = std::stoi(params[18]);
                        rod_status_.error_log_code[8] = std::stoi(params[19]);
                        rod_status_.error_log_date[8] = std::stoi(params[20]);
                        rod_status_.error_log_code[9] = std::stoi(params[21]);    
                        rod_status_.error_log_date[9] = std::stoi(params[22]);
                        mutex_rod_status_.unlock();
                    }
                    else
                    {
                        DEBUG_ERR("[tcpSockRecvHandler] Number of parameters does not match");
                    }
                }
                else
                {
                    DEBUG_ERR("[tcpSockRecvHandler] Unable to parse parameters from response_string");
                }
                mutex_get_elog_.unlock();
                break;

            /* GET RUNTIME HOURS */
            case CMDITF_REQ_GET_HOURS:
                position = response_string.find(CMDITF_CRA_GET_HOURS);
                if (position != response_string.npos)
                {
                    std::vector<std::string> params = splitString(response_string,' ');
                    if(params.size() == 3)
                    {
                        pending_resp_ = CMDITF_RESP_GET_HOURS;
                        mutex_rod_status_.lock();
                        rod_status_.rod_hours = std::stoi(params[2]);
                        mutex_rod_status_.unlock();
                    }
                    else
                    {
                        DEBUG_ERR("[tcpSockRecvHandler] Number of parameters does not match");
                    }
                }
                else
                {
                    DEBUG_ERR("[tcpSockRecvHandler] Unable to parse parameters from response_string");
                }
                mutex_get_hours_.unlock();
                break;

            /* GET WINDOW CALIBRATION */
            case CMDITF_REQ_GET_WCALIB:
                position = response_string.find(CMDITF_CRA_GET_WCALIB);
                if (position != response_string.npos)
                {
                    std::vector<std::string> params = splitString(response_string,' ');
                    if(params.size() == 3)
                    {
                        pending_resp_ = CMDITF_RESP_GET_WCALIB;
                        mutex_rod_status_.lock();
                        rod_status_.rod_window_calib = std::stoi(params[2]);
                        mutex_rod_status_.unlock();
                    }
                    else
                    {
                        DEBUG_ERR("[tcpSockRecvHandler] Number of parameters does not match");
                    }
                }
                else
                {
                    DEBUG_ERR("[tcpSockRecvHandler] Unable to parse parameters from response_string");
                }
                mutex_get_wcalib_.unlock();
                break;

            /* GET FILTER SETTING */
            case CMDITF_REQ_GET_FILTER:
                position = response_string.find(CMDITF_CRA_GET_FILTER);
                if (position != response_string.npos)
                {
                    std::vector<std::string> params = splitString(response_string,' ');
                    if(params.size() == 5)
                    {
                        pending_resp_ = CMDITF_RESP_GET_FILTER;
                        mutex_rod_status_.lock();
                        rod_status_.filter_type = std::stoi(params[2]);
                        rod_status_.filter_spots_hist = std::stoi(params[3]);
                        rod_status_.filter_spots_nghb = std::stoi(params[4]);
                        mutex_rod_status_.unlock();
                    }
                    else
                    {
                        DEBUG_ERR("[tcpSockRecvHandler] Number of parameters does not match");
                    }
                }
                else
                {
                    DEBUG_ERR("[tcpSockRecvHandler] Unable to parse parameters from response_string");
                }
                mutex_get_filter_.unlock();
                break;

            /* GET CURRENTLY OCCURING ERROR CODE */
            case CMDITF_REQ_GET_ECODE:
                position = response_string.find(CMDITF_CRA_GET_ECODE);
                if (position != response_string.npos)
                {
                    std::vector<std::string> params = splitString(response_string,' ');
                    if(params.size() == 3)
                    {
                        pending_resp_ = CMDITF_RESP_GET_ECODE;
                        mutex_rod_status_.lock();
                        rod_status_.error = std::stoi(params[2]);
                        mutex_rod_status_.unlock();
                    }
                    else
                    {
                        DEBUG_ERR("[tcpSockRecvHandler] Number of parameters does not match");
                    }
                }
                else
                {
                    DEBUG_ERR("[tcpSockRecvHandler] Unable to parse parameters from response_string");
                }
                mutex_get_ecode_.unlock();
                break;

            /* GET MDI TRANSMISSION STATUS */
            case CMDITF_REQ_GET_TXMDI:
                position = response_string.find(CMDITF_CRA_GET_TXMDI);
                if (position != response_string.npos)
                {
                    std::vector<std::string> params = splitString(response_string,' ');
                    if(params.size() == 3)
                    {
                        pending_resp_ = CMDITF_RESP_GET_TXMDI;
                        mutex_rod_status_.lock();
                        rod_status_.mdi_tx_status = std::stoi(params[2]);
                        mutex_rod_status_.unlock();
                    }
                    else
                    {
                        DEBUG_ERR("[tcpSockRecvHandler] Number of parameters does not match");
                    }
                }
                else
                {
                    DEBUG_ERR("[tcpSockRecvHandler] Unable to parse parameters from response_string");
                }
                mutex_get_txmdi_.unlock();
                break;

            default:
                DEBUG_ERR("[tcpSockRecvHandler] Unable to parse given string: " << response_string);
                pending_req_ = CMDITF_REQ_NULL;
                retval = false;
                break;
        }
        
        return retval;
    }

    /**
     *@brief    start capturing of MDI packets
     *@return   true if success
     */
    bool EthCommTCP::startCapturing()
    {
        bool retval = false;

        if(state_connected_)
        {
            state_capturing_ = true;
            retval = true;
        }
        else
        {
            state_capturing_ = false;
        }  

        return retval;
    }  

    /**
     *@brief close TCP socket and disconnect from ROD
     */
    void EthCommTCP::disconnect()
    {
        try
        {
            if(nullptr != tcp_socket_)
            {
                tcp_socket_->close();
            }
            else
            {
                DEBUG_ERR("[disconnect] No TCP socket has been opened");
            }

            io_service_.stop();
            if(boost::this_thread::get_id() != io_service_thread_.get_id())
            {
                io_service_thread_.join();
            }
            else
            {
                DEBUG_ERR("[disconnect] Unable to get an ID of io_service_thread_");
            }
        }
        catch (std::exception& e)
        {
            DEBUG_ERR("[disconnect] Exception: " <<  e.what());
        }
    }

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

    /**
     * @brief    Ask laser scanner to start to send MDI (measurement data)
     *
     * @return   0 if success
     */
    int EthCommTCP::SendMDI()
    {
        int retval = CMDITF_RETVAL_ERROR;
        boost::system::error_code ec;

        ec = tcpSockSendCommand(CMDITF_REQ_SEND_MDI);
        if(ec)
        {
            DEBUG_ERR("[SendMDI] " << boost::system::system_error(ec).what());
            retval = CMDITF_RETVAL_TX_TIMEOUT;
        }
        else
        {
            if(mutex_send_mdi_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
            {
                if(pending_resp_ == CMDITF_RESP_SEND_MDI)
                {
                    pending_resp_ = CMDITF_RESP_NULL;
                    retval = CMDITF_RETVAL_OK;
                }
                else
                {
                    retval = CMDITF_RETVAL_ERROR;
                } 
            } 
            else
            {
                retval = CMDITF_RETVAL_RX_TIMEOUT;
            } 
        } 
        return retval;
    }

    /**
     * @brief    Ask laser scanner to stop to send MDI (measurement data)
     *
     * @return   0 if success
     */
    int EthCommTCP::StopMDI()
    {
        int retval = CMDITF_RETVAL_ERROR;
        boost::system::error_code ec;

        ec = tcpSockSendCommand(CMDITF_REQ_STOP_MDI);
        if(ec)
        {
            DEBUG_ERR("[StopMDI] " << boost::system::system_error(ec).what());
            retval = CMDITF_RETVAL_TX_TIMEOUT;
        }
        else
        { 
            if(mutex_stop_mdi_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
            {
                if(pending_resp_ == CMDITF_RESP_STOP_MDI)
                {
                    pending_resp_ = CMDITF_RESP_NULL;
                    retval = CMDITF_RETVAL_OK;
                }
                else
                {
                    retval = CMDITF_RETVAL_ERROR;
                } 
            } 
            else
            {
                retval = CMDITF_RETVAL_RX_TIMEOUT;
            } 
        } 

        return retval;
    }

    /****************************************************************************************************/
    
    /**
     * @brief    Query laser scanner for its MDI transmission protocol (UDP / TCP)
     *
     * @return   0 if success
     */
    int EthCommTCP::GetProto()
    {
        int retval = CMDITF_RETVAL_ERROR;
        boost::system::error_code ec;

        ec = tcpSockSendCommand(CMDITF_REQ_GET_PROTO);
        if(ec)
        {
            DEBUG_ERR("[GetProto] " << boost::system::system_error(ec).what());
            retval = CMDITF_RETVAL_TX_TIMEOUT;
        }
        else
        { 
            if(mutex_get_proto_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
            {
                if(pending_resp_ == CMDITF_RESP_GET_PROTO)
                {
                    pending_resp_ = CMDITF_RESP_NULL;
                    retval = CMDITF_RETVAL_OK;
                }
                else
                {
                    retval = CMDITF_RETVAL_ERROR;
                } 
            } 
            else
            {
                retval = CMDITF_RETVAL_RX_TIMEOUT;
            } 
        } 

        return retval;
    }

    /**
     * @brief    Query laser scanner for its MDI data packet type (Distance only / Distance & intensity)
     *
     * @return   0 if success
     */
    int EthCommTCP::GetPType()
    {
        int retval = CMDITF_RETVAL_ERROR;
        boost::system::error_code ec;

        ec = tcpSockSendCommand(CMDITF_REQ_GET_PTYPE);
        if(ec)
        {
            DEBUG_ERR("[GetPType] " << boost::system::system_error(ec).what());
            retval = CMDITF_RETVAL_TX_TIMEOUT;
        }
        else
        { 
            if(mutex_get_ptype_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
            {
                if(pending_resp_ == CMDITF_RESP_GET_PTYPE)
                {
                    pending_resp_ = CMDITF_RESP_NULL;
                    retval = CMDITF_RETVAL_OK;
                }
                else
                {
                    retval = CMDITF_RETVAL_ERROR;
                } 
            } 
            else
            {
                retval = CMDITF_RETVAL_RX_TIMEOUT;
            } 
        } 

        return retval;
    }

    /**
     * @brief    Query laser scanner for its angular resolution (scan frequency)
     *
     * @return   0 if success
     */
    int EthCommTCP::GetResol()
    {
        int retval = CMDITF_RETVAL_ERROR;
        boost::system::error_code ec;

        ec = tcpSockSendCommand(CMDITF_REQ_GET_RESOL);
        if(ec)
        {
            DEBUG_ERR("[GetResol] " << boost::system::system_error(ec).what());
            retval = CMDITF_RETVAL_TX_TIMEOUT;
        }
        else
        { 
            if(mutex_get_resol_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
            {
                if(pending_resp_ == CMDITF_RESP_GET_RESOL)
                {
                    pending_resp_ = CMDITF_RESP_NULL;
                    retval = CMDITF_RETVAL_OK;
                }
                else
                {
                    retval = CMDITF_RETVAL_ERROR;
                } 
            } 
            else
            {
                retval = CMDITF_RETVAL_RX_TIMEOUT;
            } 
        } 

        return retval;
    }

    /**
     * @brief    Query laser scanner for its MDI data output direction (clockwise / counterclockwise)
     *
     * @return   0 if success
     */

    int EthCommTCP::GetDir()
    {
        int retval = CMDITF_RETVAL_ERROR;
        boost::system::error_code ec;

        ec = tcpSockSendCommand(CMDITF_REQ_GET_DIR);
        if(ec)
        {
            DEBUG_ERR("[GetDir] " << boost::system::system_error(ec).what());
            retval = CMDITF_RETVAL_TX_TIMEOUT;
        }
        else
        { 
            if(mutex_get_dir_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
            {
                if(pending_resp_ == CMDITF_RESP_GET_DIR)
                {
                    pending_resp_ = CMDITF_RESP_NULL;
                    retval = CMDITF_RETVAL_OK;
                }
                else
                {
                    retval = CMDITF_RETVAL_ERROR;
                } 
            } 
            else
            {
                retval = CMDITF_RETVAL_RX_TIMEOUT;
            } 
        } 

        return retval;
    }

    /**
     * @brief    Query laser scanner for its absolute angle range of one complete scan
     *
     * @return   0 if success
     */
    int EthCommTCP::GetRange()
    {
        int retval = CMDITF_RETVAL_ERROR;
        boost::system::error_code ec;

        ec = tcpSockSendCommand(CMDITF_REQ_GET_RANGE);
        if(ec)
        {
            DEBUG_ERR("[GetRange] " << boost::system::system_error(ec).what());
            retval = CMDITF_RETVAL_TX_TIMEOUT;
        }
        else
        { 
            if(mutex_get_range_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
            {
                if(pending_resp_ == CMDITF_RESP_GET_RANGE)
                {
                    pending_resp_ = CMDITF_RESP_NULL;
                    retval = CMDITF_RETVAL_OK;
                }
                else
                {
                    retval = CMDITF_RETVAL_ERROR;
                } 
            } 
            else
            {
                retval = CMDITF_RETVAL_RX_TIMEOUT;
            } 
        } 

        return retval;
    }

    /**
     * @brief    Query laser scanner for its skip spots between successive output measurements
     *
     * @return   0 if success
     */
    int EthCommTCP::GetSkip()
    {
        int retval = CMDITF_RETVAL_ERROR;
        boost::system::error_code ec;

        ec = tcpSockSendCommand(CMDITF_REQ_GET_SKIP);
        if(ec)
        {
            DEBUG_ERR("[GetSkip] " << boost::system::system_error(ec).what());
            retval = CMDITF_RETVAL_TX_TIMEOUT;
        }
        else
        { 
            if(mutex_get_skip_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
            {
                if(pending_resp_ == CMDITF_RESP_GET_SKIP)
                {
                    pending_resp_ = CMDITF_RESP_NULL;
                    retval = CMDITF_RETVAL_OK;
                }
                else
                {
                    retval = CMDITF_RETVAL_ERROR;
                } 
            } 
            else
            {
                retval = CMDITF_RETVAL_RX_TIMEOUT;
            } 
        } 

        return retval;
    }

    /**
     * @brief    Query laser scanner for its warning and error threshold percentage value of contamination
     *
     * @return   0 if success
     */
    int EthCommTCP::GetCont()
    {
        int retval = CMDITF_RETVAL_ERROR;
        boost::system::error_code ec;

        ec = tcpSockSendCommand(CMDITF_REQ_GET_CONT);
        if(ec)
        {
            DEBUG_ERR("[GetCont] " << boost::system::system_error(ec).what());
            retval = CMDITF_RETVAL_TX_TIMEOUT;
        }
        else
        { 
            if(mutex_get_cont_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
            {
                if(pending_resp_ == CMDITF_RESP_GET_CONT)
                {
                    pending_resp_ = CMDITF_RESP_NULL;
                    retval = CMDITF_RETVAL_OK;
                }
                else
                {
                    retval = CMDITF_RETVAL_ERROR;
                } 
            } 
            else
            {
                retval = CMDITF_RETVAL_RX_TIMEOUT;
            } 
        } 

        return retval;
    } 

     /**
     * @brief    Query laser scanner for its contamination state
     *
     * @return   0 if success
     */
    int EthCommTCP::GetWinStat()
    {
        int retval = CMDITF_RETVAL_ERROR;
        boost::system::error_code ec;

        ec = tcpSockSendCommand(CMDITF_REQ_GET_WINSTAT);
        if(ec)
        {
            DEBUG_ERR("[GetWinStat] " << boost::system::system_error(ec).what());
            retval = CMDITF_RETVAL_TX_TIMEOUT;
        }
        else
        { 
            if(mutex_get_winstat_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
            {
                if(pending_resp_ == CMDITF_RESP_GET_WINSTAT)
                {
                    pending_resp_ = CMDITF_RESP_NULL;
                    retval = CMDITF_RETVAL_OK;
                }
                else
                {
                    retval = CMDITF_RETVAL_ERROR;
                } 
            } 
            else
            {
                retval = CMDITF_RETVAL_RX_TIMEOUT;
            } 
        } 

        return retval;
    } 

    /**
     * @brief    Query laser scanner for its version information
     *
     * @return   0 if success
     */
    int EthCommTCP::GetVer()
    {
        int retval = CMDITF_RETVAL_ERROR;
        boost::system::error_code ec;

        ec = tcpSockSendCommand(CMDITF_REQ_GET_VER);
        if(ec)
        {
            DEBUG_ERR("[GetVer] " << boost::system::system_error(ec).what());
            retval = CMDITF_RETVAL_TX_TIMEOUT;
        }
        else
        { 
            if(mutex_get_ver_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
            {
                if(pending_resp_ == CMDITF_RESP_GET_VER)
                {
                    pending_resp_ = CMDITF_RESP_NULL;
                    retval = CMDITF_RETVAL_OK;
                }
                else
                {
                    retval = CMDITF_RETVAL_ERROR;
                } 
            } 
            else
            {
                retval = CMDITF_RETVAL_RX_TIMEOUT;
            } 
        } 

        return retval;
    } 

     /**
     * @brief    Query laser scanner for its internal temperature
     *
     * @return   0 if success
     */
    int EthCommTCP::GetTem()
    {
        int retval = CMDITF_RETVAL_ERROR;
        boost::system::error_code ec;

        ec = tcpSockSendCommand(CMDITF_REQ_GET_TEM);
        if(ec)
        {
            DEBUG_ERR("[GetTem] " << boost::system::system_error(ec).what());
            retval = CMDITF_RETVAL_TX_TIMEOUT;
        }
        else
        { 
            if(mutex_get_tem_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
            {
                if(pending_resp_ == CMDITF_RESP_GET_TEM)
                {
                    pending_resp_ = CMDITF_RESP_NULL;
                    retval = CMDITF_RETVAL_OK;
                }
                else
                {
                    retval = CMDITF_RETVAL_ERROR;
                } 
            } 
            else
            {
                retval = CMDITF_RETVAL_RX_TIMEOUT;
            } 
        } 

        return retval;
    } 

     /**
     * @brief    Query laser scanner for its internal stored error log
     *
     * @return   0 if success
     */
    int EthCommTCP::GetELog()
    {
        int retval = CMDITF_RETVAL_ERROR;
        boost::system::error_code ec;

        ec = tcpSockSendCommand(CMDITF_REQ_GET_ELOG);
        if(ec)
        {
            DEBUG_ERR("[GetELog] " << boost::system::system_error(ec).what());
            retval = CMDITF_RETVAL_TX_TIMEOUT;
        }
        else
        { 
            if(mutex_get_elog_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
            {
                if(pending_resp_ == CMDITF_RESP_GET_ELOG)
                {
                    pending_resp_ = CMDITF_RESP_NULL;
                    retval = CMDITF_RETVAL_OK;
                }
                else
                {
                    retval = CMDITF_RETVAL_ERROR;
                } 
            } 
            else
            {
                retval = CMDITF_RETVAL_RX_TIMEOUT;
            } 
        } 

        return retval;
    } 

     /**
     * @brief    Query laser scanner for its runtime hours
     *
     * @return   0 if success
     */
    int EthCommTCP::GetHours()
    {
        int retval = CMDITF_RETVAL_ERROR;
        boost::system::error_code ec;

        ec = tcpSockSendCommand(CMDITF_REQ_GET_HOURS);
        if(ec)
        {
            DEBUG_ERR("[GetHours] " << boost::system::system_error(ec).what());
            retval = CMDITF_RETVAL_TX_TIMEOUT;
        }
        else
        { 
            if(mutex_get_hours_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
            {
                if(pending_resp_ == CMDITF_RESP_GET_HOURS)
                {
                    pending_resp_ = CMDITF_RESP_NULL;
                    retval = CMDITF_RETVAL_OK;
                }
                else
                {
                    retval = CMDITF_RETVAL_ERROR;
                } 
            } 
            else
            {
                retval = CMDITF_RETVAL_RX_TIMEOUT;
            } 
        } 

        return retval;
    } 

     /**
     * @brief    Query laser scanner for its calibration status
     *
     * @return   0 if success
     */
    int EthCommTCP::GetWCalib()
    {
        int retval = CMDITF_RETVAL_ERROR;
        boost::system::error_code ec;

        ec = tcpSockSendCommand(CMDITF_REQ_GET_WCALIB);
        if(ec)
        {
            DEBUG_ERR("[GetWCalib] " << boost::system::system_error(ec).what());
            retval = CMDITF_RETVAL_TX_TIMEOUT;
        }
        else
        { 
            if(mutex_get_wcalib_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
            {
                if(pending_resp_ == CMDITF_RESP_GET_WCALIB)
                {
                    pending_resp_ = CMDITF_RESP_NULL;
                    retval = CMDITF_RETVAL_OK;
                }
                else
                {
                    retval = CMDITF_RETVAL_ERROR;
                } 
            } 
            else
            {
                retval = CMDITF_RETVAL_RX_TIMEOUT;
            } 
        } 

        return retval;
    } 

     /**
     * @brief    Query laser scanner for applied filter type
     *
     * @return   0 if success
     */
    int EthCommTCP::GetFilter()
    {
        int retval = CMDITF_RETVAL_ERROR;
        boost::system::error_code ec;

        ec = tcpSockSendCommand(CMDITF_REQ_GET_FILTER);
        if(ec)
        {
            DEBUG_ERR("[GetFilter] " << boost::system::system_error(ec).what());
            retval = CMDITF_RETVAL_TX_TIMEOUT;
        }
        else
        { 
            if(mutex_get_filter_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
            {
                if(pending_resp_ == CMDITF_RESP_GET_FILTER)
                {
                    pending_resp_ = CMDITF_RESP_NULL;
                    retval = CMDITF_RETVAL_OK;
                }
                else
                {
                    retval = CMDITF_RETVAL_ERROR;
                } 
            } 
            else
            {
                retval = CMDITF_RETVAL_RX_TIMEOUT;
            } 
        } 

        return retval;
    } 

    /**
     * @brief    Query laser scanner for error code that are currently occuring
     *
     * @return   0 if success
     */
    int EthCommTCP::GetECode()
    {
        int retval = CMDITF_RETVAL_ERROR;
        boost::system::error_code ec;

        ec = tcpSockSendCommand(CMDITF_REQ_GET_ECODE);
        if(ec)
        {
            DEBUG_ERR("[GetECode] " << boost::system::system_error(ec).what());
            retval = CMDITF_RETVAL_TX_TIMEOUT;
        }
        else
        { 
            if(mutex_get_ecode_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
            {
                if(pending_resp_ == CMDITF_RESP_GET_ECODE)
                {
                    pending_resp_ = CMDITF_RESP_NULL;
                    retval = CMDITF_RETVAL_OK;
                }
                else
                {
                    retval = CMDITF_RETVAL_ERROR;
                } 
            } 
            else
            {
                retval = CMDITF_RETVAL_RX_TIMEOUT;
            } 
        } 

        return retval;
    } 

    /**
     * @brief    Query laser scanner for current MDI transmission status
     *
     * @return   0 if success
     */
    int EthCommTCP::GetTxMDI()
    {
        int retval = CMDITF_RETVAL_ERROR;
        boost::system::error_code ec;

        ec = tcpSockSendCommand(CMDITF_REQ_GET_TXMDI);
        if(ec)
        {
            DEBUG_ERR("[GetTxMDI] " << boost::system::system_error(ec).what());
            retval = CMDITF_RETVAL_TX_TIMEOUT;
        }
        else
        { 
            if(mutex_get_txmdi_.try_lock_for(std::chrono::milliseconds(CMDITF_MSG_TIMEOUT_MS)))
            {
                if(pending_resp_ == CMDITF_RESP_GET_TXMDI)
                {
                    pending_resp_ = CMDITF_RESP_NULL;
                    retval = CMDITF_RETVAL_OK;
                }
                else
                {
                    retval = CMDITF_RETVAL_ERROR;
                } 
            } 
            else
            {
                retval = CMDITF_RETVAL_RX_TIMEOUT;
            } 
        } 

        return retval;
    } 

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

     /**
     * @brief   Get ROD status
     *
     * @return  parameterInfo structure  
     */
    leuze_msgs::msg::StatusProfileMsgRod EthCommTCP::getStatusData()
    {
        mutex_rod_status_.lock();
        auto status_data_ = rod_status_;
        mutex_rod_status_.unlock();
        return status_data_; 
    }

    /**
     * @brief   split string to multiple substrings thet are separadet by a delimiter
     * @param   input_str   = input string
     * @param   dalimiter   = delimiter
     * @return  vector splitted substring 
     */
    std::vector<std::string> EthCommTCP::splitString(const std::string &input_str, const char delimiter)
    {
        std::istringstream stream(input_str);
        std::string token;
        std::vector<std::string> tokens;

        while(std::getline(stream, token, delimiter))
        {
            tokens.push_back(token);
        }

        return tokens;
    }
}