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

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

#include "leuze_rod_driver/eth_comm_udp.h"

namespace leuze_rod_ros2_drivers 
{
    /************************************************************************************************************
     * @class   EthCommUDP
     * 
     * @brief   This class ensures UDP commmunication with a ROD scanner. It supports capturing of an UDP process
     *          data (MDI packets).
     ************************************************************************************************************/

    /**
     *@brief    constructor 
     *@param    hostname
     *@param    port
     *@param    use_udp
     *@param    inbuf_
     *@param    instream_
     *@param    ring_buffer_
     *@param    scan_data_
     */
    EthCommUDP::EthCommUDP(int port) : ScanDataParser(), inbuf_(SCANPAR_BUFF_SIZE_IN), instream_(&inbuf_)
    {
        udp_socket_ = nullptr;
        udp_port_   = -1;

        state_connected_ = false;
        
        last_scan_time_ = std::time(0);
        
        try
        {
            udp_socket_ = new boost::asio::ip::udp::socket(io_service_, boost::asio::ip::udp::v4());
            udp_socket_->bind(boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), port));
            udp_port_ = udp_socket_->local_endpoint().port();

            // start async reading
            udp_socket_->async_receive_from(boost::asio::buffer(&udp_buffer_[0], udp_buffer_.size()), udp_endpoint_,
                                            boost::bind(&EthCommUDP::udpSockReadHandler, this,
                                            boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred));
            io_service_thread_ = boost::thread(boost::bind(&boost::asio::io_service::run, &io_service_));
            state_connected_ = true;
        }
        catch (std::exception& e)
        {
            DEBUG_ERR("[EthCommUDP] Exception: " <<  e.what());
        }
    }

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

        delete udp_socket_;
        udp_socket_ = nullptr;
    }

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

    /**
     *@brief    handle receiving and parsing of UDP packets
     *@param    error
     *@param    bytes_transferred
     */
    void EthCommUDP::udpSockReadHandler(const boost::system::error_code &error, std::size_t bytes_transferred)
    {
        if(!error)
        {
            writeToRingBuff(&udp_buffer_[0], bytes_transferred);        // read received packets and write them to the internal ring buffer
            
            parsePacketsFromRingBuff();                              // read and parse packets stored in the internal ring buffer

            // asynchronous read
            udp_socket_->async_receive_from(boost::asio::buffer(&udp_buffer_[0], udp_buffer_.size()), udp_endpoint_,
                                            boost::bind(&EthCommUDP::udpSockReadHandler, this, boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred));
        }
        else
        {
            DEBUG_ERR("[udpSockReadHandler] Error (UDP): " << error.message() << "(" << error.value() << ")");
            disconnect();
        }

        last_scan_time_ = std::time(0);
    }

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

    /**
     *@brief    disconnect from opened TCP or UDP socket
     */
    void EthCommUDP::disconnect()
    {
        state_connected_ = false;
        try
        {
            if(nullptr != udp_socket_)
            {
                udp_socket_->close();
            }
            else
            {
                DEBUG_ERR("[disconnect] No UDP 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    get connection state
     *@return   connection state
     */
    bool EthCommUDP::getConnectionState()
    {
        bool retval = false;

        if(getStateConnected())
        {
            if((std::time(0) - last_scan_time_) > SCANPAR_TIMEOUT_MDI_PACKET)
            {
                DEBUG_ERR("[getConnectionState] MDI packet timeout exceed!!! Disconnecting from the scanner...");
                disconnect();
            }
            else
            {
                retval = true;
            } 
        }
        else
        {
            // false will be returned
        }  

        return retval;
    }

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

}