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

#include <chrono>
#include <ctime>
#include <memory>

#include <rclcpp/rclcpp.hpp>

#include "leuze_msgs/msg/status_profile_msg_rod.hpp"
#include "leuze_rod_driver/eth_comm_tcp.h"
#include "leuze_rod_driver/eth_comm_rod.h"
#include "leuze_rod_driver/eth_comm_udp.h"

namespace leuze_rod_ros2_drivers
{
    /************************************************************************************************************
     * @class EthCommRod
     * 
     * @brief This class ensures communication with a real ROD scanner  
     ************************************************************************************************************/

    /**
     * @brief   constructor
     */
    EthCommRod::EthCommRod()
    {
        hostname_   = "";
        port_       = 0;

        eth_comm_tcp_           = nullptr;
        state_connected_tcp_    = false;
        state_capturing_tcp_    = false;

        eth_comm_udp_           = nullptr;
        state_connected_udp_    = false;
        state_capturing_udp_    = false;

        refresh_req_id_ = CMDITF_RESP_GET_PROTO;
    }

    /**
     * @brief   destructor
     */
    EthCommRod::~EthCommRod()
    {
        if(getCapturingState())
        {
            stopCapturing();
        } 
        else 
        {
            DEBUG_WRN("[disconnect] Capturing has not been started yet");
            state_capturing_udp_ = false;
            state_capturing_tcp_ = false;
        } 
          
        closeSocketUDP();
        closeSocketTCP();
    }

    /**
     * @brief    close used TCP socket
     * @return   true if success 
     */
    bool EthCommRod::closeSocketTCP()
    {
        bool retval = true;

        if(eth_comm_tcp_ != nullptr)
        { 
            delete eth_comm_tcp_;
            eth_comm_tcp_ = nullptr;
        } 
        else
        {
            DEBUG_LOG("[closeSocketTCP] TCP communication has not been used.");
            retval = false;
        }

        state_connected_tcp_ = false;

        return retval;
    }

    /**
     * @brief    close used UDP socket
     * @return   true if success 
     */
    bool EthCommRod::closeSocketUDP()
    {
        bool retval = true;

        if(eth_comm_udp_ != nullptr)
        { 
            delete eth_comm_udp_;
            eth_comm_udp_ = nullptr;
        } 
        else
        {
            DEBUG_LOG("[closeSocketUDP] UDP communication has not been used.");
            retval = false;
        } 

        state_connected_udp_ = false;

        return retval;
    }

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

    /**
     * @brief    connect to a ROD scanner TCP interface and read its setting
     * @param    hostname
     * @param    port  
     * @return   true if success 
     */
    bool EthCommRod::connect(const std::string hostname, int port)
    {
        bool retval = false;

        hostname_ = hostname;
        port_ = port;

        if(nullptr == eth_comm_tcp_)
        {
            eth_comm_tcp_ = new EthCommTCP(hostname, port);

            if(nullptr == eth_comm_tcp_)
            {
                DEBUG_ERR("[startCapturingUDP] Unable to initialize TCP communication!");
            }
            else
            {
                if(CMDITF_RETVAL_OK == eth_comm_tcp_->StopMDI())
                { 
                    state_connected_tcp_ = true;
                    retval = true;
                }
                else
                {
                    state_connected_tcp_ = false;
                    retval =  false;
                } 
            }
        } 
        else
        {
            DEBUG_ERR("[startCapturingUDP] A TCP communication has been already initialized!");
        } 

        return retval;
    }

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

    /**
     * @brief    get ROD scanner status
     * @return   return ROD scanner status 
     */
    leuze_msgs::msg::StatusProfileMsgRod EthCommRod::getStatusData()
    {
        return eth_comm_tcp_->getStatusData();
    }

    /**
     * @brief    read complete status data from ROD scanner
     * @return   return CMDITF_RETVAL_OK if success
     */
    int EthCommRod::readStatusData()
    {
        int retval = CMDITF_RETVAL_OK;

        retval |= eth_comm_tcp_->GetProto();
        retval |= eth_comm_tcp_->GetPType();
        retval |= eth_comm_tcp_->GetResol();
        retval |= eth_comm_tcp_->GetDir();
        retval |= eth_comm_tcp_->GetRange();
        retval |= eth_comm_tcp_->GetSkip();
        retval |= eth_comm_tcp_->GetCont();
        retval |= eth_comm_tcp_->GetWinStat();
        retval |= eth_comm_tcp_->GetVer();
        retval |= eth_comm_tcp_->GetTem();
        retval |= eth_comm_tcp_->GetELog();
        retval |= eth_comm_tcp_->GetHours();
        retval |= eth_comm_tcp_->GetWCalib();
        retval |= eth_comm_tcp_->GetFilter();
        retval |= eth_comm_tcp_->GetECode();
        retval |= eth_comm_tcp_->GetTxMDI();

        /* evaluate return value */
        switch(retval)
        {
            case CMDITF_RETVAL_OK:
                break;

            case CMDITF_RETVAL_RX_TIMEOUT:
            case CMDITF_RETVAL_TX_TIMEOUT:
                DEBUG_ERR("[readStatusData] Timeout occured!");
                break;

            case CMDITF_RETVAL_ERROR:
            default:
                DEBUG_ERR("[readStatusData] Unknown error!");
                break;
        } 

        return retval;
    }

    /**
     * @brief    refresh ROD scanner status data
     */
    int EthCommRod::refreshStatusData()
    {
        int retval = CMDITF_RETVAL_OK;

        /* send a request to refresh status data */
        switch(refresh_req_id_)
        {
            case CMDITF_RESP_GET_PROTO:
                retval = eth_comm_tcp_->GetProto();
                refresh_req_id_ = CMDITF_RESP_GET_PTYPE;
                break;

            case CMDITF_RESP_GET_PTYPE:
                retval = eth_comm_tcp_->GetPType();
                refresh_req_id_ = CMDITF_RESP_GET_RESOL;
                break;
                
            case CMDITF_RESP_GET_RESOL:
                retval = eth_comm_tcp_->GetResol();
                refresh_req_id_ = CMDITF_RESP_GET_DIR;
                break;
                
            case CMDITF_RESP_GET_DIR:
                retval = eth_comm_tcp_->GetDir();
                refresh_req_id_ = CMDITF_RESP_GET_RANGE;
                break;
                
            case CMDITF_RESP_GET_RANGE:
                retval = eth_comm_tcp_->GetRange();
                refresh_req_id_ = CMDITF_RESP_GET_SKIP;
                break;
                
            case CMDITF_RESP_GET_SKIP:
                retval = eth_comm_tcp_->GetSkip();
                refresh_req_id_ = CMDITF_RESP_GET_CONT;     
                break;
                
            case CMDITF_RESP_GET_CONT:
                retval = eth_comm_tcp_->GetCont();
                refresh_req_id_ = CMDITF_RESP_GET_WINSTAT;
                break;
                
            case CMDITF_RESP_GET_WINSTAT:
                retval = eth_comm_tcp_->GetWinStat();
                refresh_req_id_ = CMDITF_RESP_GET_VER;
                break;
                
            case CMDITF_RESP_GET_VER:
                retval = eth_comm_tcp_->GetVer();
                refresh_req_id_ = CMDITF_RESP_GET_TEM;
                break;
                
            case CMDITF_RESP_GET_TEM:
                retval = eth_comm_tcp_->GetTem();
                refresh_req_id_ = CMDITF_RESP_GET_ELOG;
                break;
                
            case CMDITF_RESP_GET_ELOG:
                retval = eth_comm_tcp_->GetELog();
                refresh_req_id_ = CMDITF_RESP_GET_HOURS;
                break;
                
            case CMDITF_RESP_GET_HOURS:
                retval = eth_comm_tcp_->GetHours();
                refresh_req_id_ = CMDITF_RESP_GET_WCALIB;
                break;
                
            case CMDITF_RESP_GET_WCALIB:
                retval = eth_comm_tcp_->GetWCalib();
                refresh_req_id_ = CMDITF_RESP_GET_FILTER;
                break;
                
            case CMDITF_RESP_GET_FILTER:
                retval = eth_comm_tcp_->GetFilter();
                refresh_req_id_ = CMDITF_RESP_GET_ECODE;
                break;
                
            case CMDITF_RESP_GET_ECODE:
                retval = eth_comm_tcp_->GetECode();
                refresh_req_id_ = CMDITF_RESP_GET_TXMDI;
                break;
                
            case CMDITF_RESP_GET_TXMDI:
                retval = eth_comm_tcp_->GetTxMDI();
                refresh_req_id_ = CMDITF_RESP_GET_PROTO;
                break;
                
            default:
                retval = CMDITF_RETVAL_ERROR;
                refresh_req_id_ = CMDITF_RESP_GET_PROTO;
                break;
        }

        /* evaluate return value */
        switch(retval)
        {
            case CMDITF_RETVAL_OK:
                // OK
                break;

            case CMDITF_RETVAL_RX_TIMEOUT:
            case CMDITF_RETVAL_TX_TIMEOUT:
                DEBUG_ERR("[refreshStatusData] Timeout occured!");
                break;

            case CMDITF_RETVAL_ERROR:
            default:
                DEBUG_ERR("[refreshStatusData] Unknown error!");
                break;
        } 

        return retval;
    }

    /**
     * @brief   send a TCP command
     * @param   cmd_id
     * @return  0 if success
     */
    int EthCommRod::sendTcpCmd(std::uint32_t cmd_id)
    {
        int retval = CMDITF_RETVAL_OK;

        /* send a request to refresh status data */
        switch(cmd_id)
        {
            case CMDITF_REQ_SEND_MDI:
                retval = eth_comm_tcp_->SendMDI();
                break;

            case CMDITF_REQ_STOP_MDI:
                retval = eth_comm_tcp_->StopMDI();
                break;

            case CMDITF_REQ_GET_PROTO:
                retval = eth_comm_tcp_->GetProto();
                break;

            case CMDITF_REQ_GET_PTYPE:
                retval = eth_comm_tcp_->GetPType();
                break;

            case CMDITF_REQ_GET_RESOL:
                retval = eth_comm_tcp_->GetResol();
                break;

            case CMDITF_REQ_GET_DIR:
                retval = eth_comm_tcp_->GetDir();
                break;

            case CMDITF_REQ_GET_RANGE:
                retval = eth_comm_tcp_->GetRange();
                break;

            case CMDITF_REQ_GET_SKIP:
                retval = eth_comm_tcp_->GetSkip();
                break;

            case CMDITF_REQ_GET_CONT:
                retval = eth_comm_tcp_->GetCont();
                break;

            case CMDITF_REQ_GET_WINSTAT:
                retval = eth_comm_tcp_->GetWinStat();
                break;

            case CMDITF_REQ_GET_VER:
                retval = eth_comm_tcp_->GetVer();
                break;

            case CMDITF_REQ_GET_TEM:
                retval = eth_comm_tcp_->GetTem();
                break;

            case CMDITF_REQ_GET_ELOG:
                retval = eth_comm_tcp_->GetELog();
                break;

            case CMDITF_REQ_GET_HOURS:
                retval = eth_comm_tcp_->GetHours();
                break;

            case CMDITF_REQ_GET_WCALIB:
                retval = eth_comm_tcp_->GetWCalib();
                break;

            case CMDITF_REQ_GET_FILTER:
                retval = eth_comm_tcp_->GetFilter();
                break;

            case CMDITF_REQ_GET_ECODE:
                retval = eth_comm_tcp_->GetECode();
                break;

            case CMDITF_REQ_GET_TXMDI: 
                retval = eth_comm_tcp_->GetTxMDI();
                break;

            default:
                retval = CMDITF_RETVAL_ERROR;
                break;
        }

        /* evaluate return value */
        switch(retval)
        {
            case CMDITF_RETVAL_OK:
                // OK
                break;

            case CMDITF_RETVAL_RX_TIMEOUT:
            case CMDITF_RETVAL_TX_TIMEOUT:
                DEBUG_ERR("[sendTcpCmd] Timeout occured!");
                break;

            case CMDITF_RETVAL_ERROR:
            default:
                DEBUG_ERR("[sendTcpCmd] Unknown error!");
                break;
        } 

        return retval;
    }

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

    /**
     * @brief    start capturing of TCP measurement packets
     * @return   true if success 
     */
    bool EthCommRod::startCapturingTCP()
    {
        bool retval = false;

        DEBUG_LOG("[startCapturingTCP] ...");

        if(CMDITF_RETVAL_OK == eth_comm_tcp_->SendMDI())
        {
            if(eth_comm_tcp_->startCapturing())
            {
                state_capturing_tcp_ = true;
                retval = true;
            }
            else
            {
                DEBUG_ERR("[startCapturingTCP] Unable to start capturing of TCP scan data!");
            }
        } 
        else
        {
            DEBUG_ERR("[startCapturingTCP] Unable to start capturing of TCP scan data!");
        }    
        
        return retval;
    }

    /**
     * @brief    start capturing of UDP measurement packets
     * @return   true if success
     */
    bool EthCommRod::startCapturingUDP()
    {
        bool retval = false ;

        DEBUG_LOG("[startCapturingUDP] ...");
        
        eth_comm_tcp_->SendMDI();

        if(nullptr == eth_comm_udp_)
        {
            eth_comm_udp_ = new EthCommUDP(port_);
            if(nullptr == eth_comm_udp_)
            {
                DEBUG_ERR("[startCapturingUDP] Unable to initialize UDP communication!");
            }
            else
            {
                if(eth_comm_udp_->getStateConnected())
                {
                    state_capturing_udp_ = true;
                    retval = true;
                }
                else
                {   
                    DEBUG_ERR("[startCapturingUDP] Unable to start capturing of UDP scan data!");
                }
            }
        }
        else
        {
            DEBUG_ERR("[startCapturingUDP] A UDP communication has been already initialized!");
        }

        return retval;
    }

    /**
     * @brief    stop capturing of packets
     * @return   state of capturing
     */
    bool EthCommRod::stopCapturing()
    {
        bool retval = false;

        DEBUG_LOG("[stopCapturing] ...");

        if(state_capturing_tcp_ || state_capturing_udp_)
        {
            if(CMDITF_RETVAL_OK == eth_comm_tcp_->StopMDI())
            {
                state_capturing_tcp_ = false;
                state_capturing_udp_ = false;
                retval = true;
            }
            else
            {
                DEBUG_WRN("[stopCapturing] Unable to stop MDI!");
            }  
        }
        else
        {
            DEBUG_WRN("[stopCapturing] Capturing has not been started yet!");
        }              

        return retval;
    }

    /**
     * @brief    get packet capturing state
     * @return   state of capturing
     */
    bool EthCommRod::getCapturingState()
    {
        bool retval = false;

        if(state_capturing_tcp_ && !state_capturing_udp_)
        {
            retval = true;
        } 
        else if (!state_capturing_tcp_ && state_capturing_udp_)
        {
            if(nullptr != eth_comm_udp_)
            {
                retval = eth_comm_udp_->getConnectionState();
            }
            else
            {
                // nothing to do
            }
        }
        else
        {
            retval = false;
        }

        return retval;
    }

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

    /**
     * @brief    get scan data
     * @return   scan data 
     */
    ScanData EthCommRod::getScan()
    {
        ScanData scan_data;

        if(nullptr != eth_comm_udp_)
        {
            scan_data = eth_comm_udp_->getScan();
        }
        else if(nullptr != eth_comm_tcp_)
        {
            scan_data = eth_comm_tcp_->getScan();
        }
        else
        {
            DEBUG_ERR("[getScan] Scan capturing has not been started!");
        }

        return scan_data;
    }

    /**
     * @brief    get data of full scan
     * @return   scan data 
     */
    ScanData EthCommRod::getCompleteScan()
    {
        ScanData scan_data;

        if(nullptr != eth_comm_udp_)
        {
            scan_data = eth_comm_udp_->getCompleteScan();
        }
        else if(nullptr != eth_comm_tcp_)
        {
            scan_data = eth_comm_tcp_->getCompleteScan();
        }
        else
        {
            DEBUG_ERR("[getCompleteScan] Scan capturing has not been started!");
        }

        return scan_data;
    }

    /**
     * @brief    get number of available scans
     * @return   number of available scans 
     */
    std::size_t EthCommRod::getScansAvailable() const
    {
        std::size_t scans_available = 0;

        if(nullptr != eth_comm_udp_)
        {
            scans_available = eth_comm_udp_->getScansAvailable();
        }
        else if(nullptr != eth_comm_tcp_)
        {
            scans_available = eth_comm_tcp_->getScansAvailable();
        }
        else
        {
            DEBUG_ERR("[getScansAvailable] Scan capturing has not been started!");
        }

        return scans_available;
    }

    /**
     * @brief    get number of available full scans
     * @return   number of available full scans 
     */
    std::size_t EthCommRod::getCompleteScansAvailable() const
    {
        std::size_t scans_available = 0;

        if(nullptr != eth_comm_udp_)
        {
            scans_available = eth_comm_udp_->getCompleteScansAvailable();
        }
        else if(nullptr != eth_comm_tcp_)
        {
            scans_available = eth_comm_tcp_->getCompleteScansAvailable();
        }
        else
        {
            DEBUG_ERR("[getCompleteScansAvailable] Scan capturing has not been started!");
        }

        return scans_available;
    }
}
