/*  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_sim.h"
#include "leuze_rod_driver/eth_comm_tcp.h"

namespace leuze_rod_ros2_drivers
{
    /************************************************************************************************************
     * @class EthCommSim
     * 
     * @brief This class ensures simulation of communication with a ROD scanner 
     ************************************************************************************************************/

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

        state_connected_    = false;
        state_capturing_    = false;
        last_scan_time_     = std::chrono::system_clock::now();
        scan_length_avg_    = 1000;
        scan_offs_          = 0;

        /* MDI info */
        rod_status_.mdi_protocol            = RODPAR_PROTO_TCP;
        rod_status_.mdi_packet_type         = RODPAR_PTYPE_DISTANCE_AND_AMPLITUDE;
        rod_status_.mdi_data_direction      = RODPAR_DIR_CLOCKWISE;
        rod_status_.mdi_tx_status           = 0;

        /* Scan info */
        rod_status_.scan_resolution         = RODPAR_RESOL_0200_80_Hz;
        rod_status_.scan_angle_min          = RODRNG_ANGLE_MIN;
        rod_status_.scan_angle_max          = RODRNG_ANGLE_MAX;
        rod_status_.scan_skip_spots         = 0;

        /* Contamination info */
        rod_status_.cont_threshold_warn     = RODRNG_CONT_MAX - 10;
        rod_status_.cont_threshold_error    = RODRNG_CONT_MAX;
        for(int i = 0; i < 9; i++)
        {
            rod_status_.cont_state[i]       = i;
        }

        /* ROD scanner info */
        rod_status_.rod_part_number         = 123456789;
        rod_status_.rod_hw_ver              = 123;
        rod_status_.rod_sw_ver              = 123;
        rod_status_.rod_temp                = 2000;
        rod_status_.rod_hours               = 0;
        rod_status_.rod_window_calib        = 0;

        /* Filter */
        rod_status_.filter_type             = RODPAR_FILTER_MEDIAN;
        rod_status_.filter_spots_hist       = 0;
        rod_status_.filter_spots_nghb       = 0;

        /* Errors */
        rod_status_.error                   = 0;       
        for(int i = 0; i < 10; i ++)
        {
            rod_status_.error_log_code[i] = 100 + i;
            rod_status_.error_log_date[i] = i;
        } 

        /* calculate number of scan spots and scan time spacing */
        switch (rod_status_.scan_resolution)
        {
            case RODPAR_RESOL_0200_80_Hz: 
                scan_spots_     = std::size_t(((RODRNG_ANGLE_MAX - RODRNG_ANGLE_MIN) / 100) / 0.200);
                scan_period_    = std::chrono::milliseconds(12);
                break;

            case RODPAR_RESOL_0200_50_Hz:

                scan_spots_     = std::size_t(((RODRNG_ANGLE_MAX - RODRNG_ANGLE_MIN) / 100) / 0.200);
                scan_period_    = std::chrono::milliseconds(12);
                break;

            case RODPAR_RESOL_0100_40_Hz:
                scan_spots_     = std::size_t(((RODRNG_ANGLE_MAX - RODRNG_ANGLE_MIN) / 100) / 0.100);
                scan_period_    = std::chrono::milliseconds(20);
                break;

            case RODPAR_RESOL_0050_20_Hz:
                scan_spots_     = std::size_t(((RODRNG_ANGLE_MAX - RODRNG_ANGLE_MIN) / 100) / 0.050);
                scan_period_    = std::chrono::milliseconds(25);
                break;

            case RODPAR_RESOL_0025_10_Hz:
                scan_spots_     = std::size_t(((RODRNG_ANGLE_MAX - RODRNG_ANGLE_MIN) / 100) / 0.025);
                scan_period_    = std::chrono::milliseconds(100);
                break;
        
            default:
                scan_spots_     = 0;
                scan_period_    = std::chrono::milliseconds(1000);
                break;
        }
    }

    /**
     * @brief  destructor
     */
    EthCommSim::~EthCommSim()
    {
        state_connected_    = false;
        state_capturing_    = false;
    }

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

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

        return true;
    }

    /**
     * @brief    close used TCP socket
     * @return   true if success 
     */
    bool EthCommSim::closeSocketTCP()
    {
        return true;
    }

    /**
     * @brief    close used UDP socket
     * @return   true if success 
     */
    bool EthCommSim::closeSocketUDP()
    {
        return true;
    }

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

    /**
     * @brief    get ROD scanner status
     * @return   return ROD scanner status 
     */
    leuze_msgs::msg::StatusProfileMsgRod EthCommSim::getStatusData()
    {
        return rod_status_;
    }

    /**
     * @brief    load complete ROD scanner status data
     */
    int EthCommSim::readStatusData()
    {
        return CMDITF_RETVAL_OK;
    }

    /**
     * @brief    refresh ROD scanner status data
     */
    int EthCommSim::refreshStatusData()
    {
        /* simulate temperature rise after power up + small temperature changes */
        if(rod_status_.rod_temp <= 5000)
        {
            rod_status_.rod_temp += 10;
        }
        else
        {
            rod_status_.rod_temp -= 100;
        } 

        return CMDITF_RETVAL_OK;
    }

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

        switch(cmd_id)
        {
            case CMDITF_REQ_SEND_MDI:
            case CMDITF_REQ_STOP_MDI:
            case CMDITF_REQ_GET_PROTO:
            case CMDITF_REQ_GET_PTYPE:
            case CMDITF_REQ_GET_RESOL:
            case CMDITF_REQ_GET_DIR:
            case CMDITF_REQ_GET_RANGE:
            case CMDITF_REQ_GET_SKIP:
            case CMDITF_REQ_GET_CONT:
            case CMDITF_REQ_GET_WINSTAT:
            case CMDITF_REQ_GET_VER:
            case CMDITF_REQ_GET_TEM:
            case CMDITF_REQ_GET_ELOG:
            case CMDITF_REQ_GET_HOURS:
            case CMDITF_REQ_GET_WCALIB:
            case CMDITF_REQ_GET_FILTER:
            case CMDITF_REQ_GET_ECODE:
            case CMDITF_REQ_GET_TXMDI: 
                break;

            default:
                retval = CMDITF_RETVAL_ERROR;
                break;
        }

        return retval;
    }

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

    /**
     * @brief    start capturing of TCP measurement packets
     * @return   true if success 
     */
    bool EthCommSim::startCapturingTCP()
    {
        DEBUG_LOG("[startCapturingTCP] ...");

        state_capturing_ = true;   
        
        return true;
    }

    /**
     * @brief    start capturing of UDP measurement packets
     * @return   true if success
     */
    bool EthCommSim::startCapturingUDP()
    {
        DEBUG_LOG("[startCapturingUDP] ...");

        state_capturing_ = true;   
        
        return true;
    }

    /**
     * @brief    stop capturing of packets
     * @return   state of capturing
     */
    bool EthCommSim::stopCapturing()
    {
        DEBUG_LOG("[stopCapturing] ...");

        state_capturing_ = false;   
                
        return false;
    }

    /**
     * @brief    get packet capturing state
     * @return   state of capturing
     */
    bool EthCommSim::getCapturingState()
    {
        return state_capturing_;
    }

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

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

        scan_data.distances.resize(scan_spots_);
        scan_data.intensities.resize(scan_spots_);

        if (scan_length_avg_ < 3000)
        {
            scan_length_avg_ += 5;
        }
        else
        {
            scan_length_avg_ = 1000;
        }

        for(std::size_t i = 0; i < scan_spots_; i++)
        {
            scan_data.distances[i]      = scan_length_avg_ + (50 * std::sin(i + scan_offs_));
            scan_data.intensities[i]    = 2048 + (128 * std::sin(i + scan_offs_));

            if(scan_offs_ < scan_spots_)
            {
                scan_offs_++;
            }
            else
            {
                scan_offs_ = 0;
            }
        }

        return scan_data;
    }

    /**
     * @brief    get data of full scan
     * @return   scan data 
     */
    ScanData EthCommSim::getCompleteScan()
    {
        return getScan();
    }

    /**
     * @brief    get number of available scans
     * @return   number of available scans 
     */
    std::size_t EthCommSim::getScansAvailable() const
    {
        std::size_t scans_available;
        std::chrono::system_clock::time_point current_time = std::chrono::system_clock::now();

        if(current_time > (last_scan_time_ + scan_period_))
        {   
            scans_available = 1;
            last_scan_time_ = current_time;
        }
        else
        {
            scans_available = 0;
        }

        return scans_available;
    }

    /**
     * @brief    get number of available full scans
     * @return   number of available full scans 
     */
    std::size_t EthCommSim::getCompleteScansAvailable() const
    {
        return getScansAvailable();
    }
}
